#include <stdio.h>   // NULL
#include <stdlib.h>  // atol()
#include <string.h>  // strlen()
#include <ctype.h>   // isspace()
#include <unistd.h>
#include "rtapi/rtapi.h"
#include "config.h"
#include "nml_intf/zuc.hh"
#include "zuc/nml_intf/zucpos.h"  // ZucPose
#include "rcs_print.hh"
#include "posemath.h"        // PM_POSE, PM_RPY
#include "initraj.hh"        // these decls
#include "nml_intf/zucglb.h" /*! \todo TRAVERSE_RATE (FIXME) */
#include "kinematics/kinematics.h"
// #include "filter.h"
// #include "zucIniFile.hh"
// #include "minIni.h"

#include "jkutil/iniparser.h"

#include <map>
#include <vector>
#include <string>
#include "motion/safezone.h"
#include <iostream>
#include "errcode.h"
#include "inisafezone.hh"
#include "iniextio.hh"
#include "inirobmodule.hh"

#include "zuc/task/motion_al.h"
#include "zuc/task/taskintf_config.h"
#include "zuc/task/taskintf.h"

extern double zucTrajGetMaxJointJerk(void);

extern double zucTrajGetMaxCarteJerk(void);

static bool IsBitSet(int data, int index) { return (data & (1 << index)) != 0; }

static void SetBit(int& data, int index) { data |= 1 << index; }

static void ClearBit(int& data, int index) { data &= ~(1 << index); }

static int syncFileToDisk(const char* filename)
{
    // int fd;
    // if (filename == NULL || (fd = open(filename, O_RDWR | O_CREAT)) == -1)
    // {
    //     return -1;
    // }

    // fsync(fd);
    // close(fd);
    return 0;
}

static int initAutoWork(const char* filepath)
{
    char autofile[256];
    const char* autoProgPre = "/usr/etc/jkzuc/scripts/program/";
    const char* defaultAutoProg = "/usr/etc/jkzuc/scripts/program/none/none.jks";
    const size_t autoProgPreLen = strlen(autoProgPre);
    try
    {
        int enable = 0;
        int savePath = 0;
        Util::IniParser ini_file(filepath);
        std::string str_value;
        ini_file.get("TASK.DEFAULT_ENABLE", (bool&)enable, false);
        ini_file.get("TASK.DEFAULT_PROG", str_value, defaultAutoProg);
        strncpy(autofile, str_value.c_str(), sizeof(autofile) / sizeof(char));
        rtapi_print("[initraj_test] TASK.DEFAULT_ENABLE:%d ,TASK.DEFAULT_PROG:%s\n", enable, autofile);

        // 当校验到传入的文件路径前缀与默认程序路径前缀不符或program后未接任何内容时，写入默认程序路径
        if (strncmp(autofile, defaultAutoProg, autoProgPreLen) != 0 || strlen(autofile) <= autoProgPreLen)
        {
            savePath = 1;
            strncpy(autofile, defaultAutoProg, 256);
        }

        // zucTaskSetAutoWork(enable, autofile, savePath);
    }
    catch (...)
    {
        // return -1;
    }

    return 0;
}

static int initOnekeyset(const char* filepath)
{
    int autoenable, autoprogram;
    try
    {
        Util::IniParser ini_file(filepath);
        ini_file.get("TASK.AUTO_ENABLE", (bool&)autoenable, false);
        ini_file.get("TASK.AUTO_PROGRAM", (bool&)autoprogram, false);
        rtapi_print("[initraj_test] TASK.AUTO_ENABLE:%d ,TASK.AUTO_PROGRAM:%f\n", autoenable, autoprogram);
        zucOneKeyOp(autoenable, autoprogram, 0);
    }
    catch (...)
    {
        // return -1;
    }

    return 0;
}

static int initScriptBackupSettings(const char* inifile)
{
    if (inifile == NULL)
        return -1;
    Util::IniParser ini_file(inifile);
    int enable = 0;
    double interval = 0.0;
    ini_file.get("SCRIPTAUTOBACKUP.ENABLE", (bool&)enable, false);
    ini_file.get("SCRIPTAUTOBACKUP.INTERVAL", interval, 15.0, 0.0, 24 * 3600 * 1.0);
    rtapi_print("[initraj_test] SCRIPTAUTOBACKUP.ENABLE:%d ,SCRIPTAUTOBACKUP.INTERVAL:%f\n", enable, interval);
    zucSetupScriptBackup(enable, interval, 0);
    return 0;
}

static int initTrajSmooth(const char* inifile)
{
    if (inifile == NULL)
        return -1;
    Util::IniParser ini_file(inifile);
    int trqLimitActive = 0;
    int firFltActive = 0;
    ini_file.get("TRAJ_SMOOTH.TRQ_LIMIT_ENABLE", (bool&)trqLimitActive, false);
    ini_file.get("TRAJ_SMOOTH.FLT_ENABLE", (bool&)firFltActive, false);
    rtapi_print("[initraj_test] get TRAJ_SMOOTH.TRQ_LIMIT_ENABLE:%d\n", trqLimitActive);
    rtapi_print("[initraj_test] get TRAJ_SMOOTH.FLT_ENABLE:%d\n", firFltActive);
    return 0;
}

int initSafetyCRC(const char* filepath)
{
    unsigned int safetyCRC;
    Util::IniParser ini_file(filepath);
    ini_file.get("SAFETY.CHECKSUM", (int32_t&)safetyCRC, 0);
    rtapi_print("[initcrc]  get crc: %#x", safetyCRC);
    zucSafetyCrc(safetyCRC);
    return 0;
}

static int initExtIOSetup()
{
    ExtIOSetup extIOSetups[MAX_EXTIO_CNT];
    memset(extIOSetups, 0, sizeof(extIOSetups));
    ExtioConfig config;
    int extIOModCnt = config.loadExtIoSetup(ZUC_USER_CONFIG_DIR "/extioconfig.xml", extIOSetups, MAX_EXTIO_CNT);
    if (extIOModCnt > 0)
    {
        zucExtIOSetup(extIOModCnt, extIOSetups, 0);
    }
    int reconnect_interval = 0, reconnect_maxCnt = 0;
    if (!config.iniLoadExtioReconnectParams(ZUC_USER_CONFIG_DIR "/usersettings.ini", &reconnect_interval, &reconnect_maxCnt))
    {
        zucExtIOSetReconnectParams(reconnect_interval, reconnect_maxCnt, 0);
    }

    return 0;
}

static int initOperationMode(Util::IniParser* ini_file)
{
    // 默认不启用手自动功能, 启用功能后默认以自动模式启动
    auto init_operation_mode = mot::al::MotionProxy::instance().robot(0);
    if (!init_operation_mode.get())
    {
        return -1;
    }
    int enable = 0;
    ini_file->get("OPERATIONMODE.ENABLE", enable, 0);
    if (0 != init_operation_mode->zucSetOperationModeOption(enable))
    {
        return -1;
    }
    if (enable == 1)
    {
        int init_mode = 0;
        ini_file->get("OPERATIONMODE.MODE", init_mode, 0);
        if (0 != zucSetOperationMode(init_mode))
        {
            return -1;
        }
        rtapi_print("[initOperationMode]  get OPERATIONMODE.MODE: %d", init_mode);
    }
    return 0;
}

static int initDHCom(Util::IniParser* ini_file)
{
    auto robot_motion_task = mot::al::MotionProxy::instance().robot(0);
    if (!robot_motion_task.get())
    {
        return -1;
    }
    int full_dh_com;
    //trajInifile->EnableExceptions(ZucIniFile::ERR_CONVERSION);
    ini_file->get("ZUC.FULL_DH_COM", full_dh_com, 0, 0, 1);
    rtapi_print("[initraj_test]  get ZUC.FULL_DH_COM: %d", full_dh_com);
    // trajInifile->Find(&full_dh_com, "FULL_DH_COM", "ZUC");
    // rcs_print("initDHCom  called  full_dh_com = %d\n", full_dh_com);

    if (0 != robot_motion_task->zucSetDHCom(full_dh_com))
    {
        return -1;
    }
    return 0;
}

static int initSetTrackingConfig(Util::IniParser* ini_file)
{
    auto robot_motion_task = mot::al::MotionProxy::instance().robot(0);
    if (!robot_motion_task.get())
    {
        return -1;
    }
    int linkdi = 0, comcyclenum = 0, lctsynch = 0;
    double synchacc = 2000.0;
    //trajInifile->EnableExceptions(ZucIniFile::ERR_CONVERSION);
    ini_file->get("TRACKING.LINK_DI", linkdi, 0, -1, 500);
    rtapi_print("[initraj_test]  get TRACKING.LINK_DI: %d", linkdi);
    ini_file->get("TRACKING.COM_CYCLE_NUM", comcyclenum, 6, 0, 125);
    rtapi_print("[initraj_test]  get TRACKING.COM_CYCLE_NUM: %d", comcyclenum);
    ini_file->get("TRACKING.SYNCH_ACC", synchacc, 2000.0, 0.0, 7200.0);
    rtapi_print("[initraj_test]  get TRACKING.SYNCH_ACC: %d", synchacc);
    ini_file->get("TRACKING.LCTSYNCH", lctsynch, 0, 0, 1);
    rtapi_print("[initraj_test]  get TRACKING.LCTSYNCH: %f", lctsynch);
    // trajInifile->Find(&linkdi, "LINK_DI", "TRACKING");
    // rcs_print(" ########################################### initSetTrackingConfig  called  linkdi = %d\n", linkdi);
    // trajInifile->Find(&comcyclenum, "COM_CYCLE_NUM", "TRACKING");
    // rcs_print(" ########################################### initSetTrackingConfig  called  comcyclenum = %d\n", comcyclenum);
    // trajInifile->Find(&synchacc, "SYNCH_ACC", "TRACKING");
    // rcs_print(" ########################################### initSetTrackingConfig  called  synchacc = %f\n", synchacc);
    // trajInifile->Find(&lctsynch, "LCTSYNCH", "TRACKING");
    // rcs_print(" ########################################### initSetTrackingConfig  called  lctsynch = %d\n", lctsynch);

    if (0 != robot_motion_task->zucTrackingConfig(linkdi, comcyclenum, synchacc, lctsynch))
    {
        return -1;
    }
    return 0;
}

static int initLimitLinearVelocity(Util::IniParser* ini_file)
{
    auto robot_motion_task = mot::al::MotionProxy::instance().robot(0);
    if (!robot_motion_task.get())
    {
        return -1;
    }
    int enable = 1;
    //trajInifile->EnableExceptions(ZucIniFile::ERR_CONVERSION);
    ini_file->get("ZUC.LIMIT_LINEAR_VELOCITY", enable, 0, 0, 1);
    rtapi_print("[initraj_test]  get ZUC.LIMIT_LINEAR_VELOCITY: %d", enable);
    // trajInifile->Find(&enable, "LIMIT_LINEAR_VELOCITY", "ZUC");
    // rcs_print("initLimitLinearVelocity  called  enable = %d\n", enable);
    if (0 != robot_motion_task->zucSetLimitLinearVelocity(enable))
    {
        return -1;
    }
    return 0;
}

static int initLimitJointVelocity(Util::IniParser* ini_file)
{
    auto robot_motion_task = mot::al::MotionProxy::instance().robot(0);
    if (!robot_motion_task.get())
    {
        return -1;
    }
    int enable = 1;
    //trajInifile->EnableExceptions(ZucIniFile::ERR_CONVERSION);
    ini_file->get("ZUC.LIMIT_JOINT_VELOCITY", enable, 0, 0, 1);
    rtapi_print("[initraj_test]  get ZUC.LIMIT_JOINT_VELOCITY: %d", enable);
    // trajInifile->Find(&enable, "LIMIT_JOINT_VELOCITY", "ZUC");
    // rcs_print("initLimitJointVelocity  called  enable = %d\n", enable);
    if (0 != robot_motion_task->zucSetLimitJointVelocity(enable))
    {
        return -1;
    }
    return 0;
}

static int initSetPlannnerJerk(Util::IniParser* ini_file)
{
    auto robot_motion_task = mot::al::MotionProxy::instance().robot(0);
    if (!robot_motion_task.get())
    {
        return -1;
    }
    double joint_jerk = 0.0, carte_jerk = 0.0;
    ini_file->get("TRAJ.TRAJ_MAX_JOINT_JERK", joint_jerk, 10000, 0, 20000);
    rtapi_print("[initraj_test]  get TRAJ.TRAJ_MAX_JOINT_JERK: %f", joint_jerk);
    ini_file->get("TRAJ.TRAJ_MAX_CARTE_JERK", carte_jerk, 50000, 0, 100000);
    rtapi_print("[initraj_test]  get TRAJ.TRAJ_MAX_CARTE_JERK: %f", carte_jerk);
    //trajInifile->Find(&joint_jerk, "TRAJ_MAX_JOINT_JERK", "TRAJ");
    //rcs_print("initSetPlannnerJerk  called  joint_jerk = %f\n", joint_jerk);
    //trajInifile->Find(&carte_jerk, "TRAJ_MAX_CARTE_JERK", "TRAJ");
    //rcs_print("initSetPlannnerJerk  called  carte_jerk = %f\n", carte_jerk);

    if (0 != robot_motion_task->zucSetPlannerJerk(joint_jerk, carte_jerk))
    {
        return -1;
    }
    return 0;
}

static int initAttitudeSafety(Util::IniParser* ini_file)
{
    int choice = 0, enable_open = 0, enable_auto = 0;
    double tilt = 0, pan = 0, deviation = 0, deviation_warning = 0;
    double cenrpy[3] = {0};
    ini_file->get("ROBOTSAFETYSETTING.CONICAL_TIP_CHOICE", choice, 0, 0, 1);
    rtapi_print("[initraj_test]  get ROBOTSAFETYSETTING.CONICAL_TIP_CHOICE: %d", choice);
    ini_file->get("ROBOTSAFETYSETTING.TOOL_PROPERTIES_TILT", tilt, 0, -360, 360);
    rtapi_print("[initraj_test]  get ROBOTSAFETYSETTING.TOOL_PROPERTIES_TILT: %f", tilt);
    ini_file->get("ROBOTSAFETYSETTING.TOOL_PROPERTIES_PAN", pan, 0, -360, 360);
    rtapi_print("[initraj_test]  get ROBOTSAFETYSETTING.TOOL_PROPERTIES_PAN: %f", pan);
    ini_file->get("ROBOTSAFETYSETTING.DEVIATION", deviation, 0, -360, 360);
    rtapi_print("[initraj_test]  get ROBOTSAFETYSETTING.DEVIATION: %f", deviation);
    ini_file->get("ROBOTSAFETYSETTING.DEVIATION_WARNING", deviation_warning, 0, -360, 360);
    rtapi_print("[initraj_test]  get ROBOTSAFETYSETTING.DEVIATION_WARNING: %f", deviation_warning);
    ini_file->get("ROBOTSAFETYSETTING.LIMIT_ATTITUDE_OPEN", enable_open, 0, 0, 1);
    rtapi_print("[initraj_test]  get ROBOTSAFETYSETTING.LIMIT_ATTITUDE_OPEN: %d", enable_open);
    ini_file->get("ROBOTSAFETYSETTING.LIMIT_ATTITUDE_AUTO", enable_auto, 0, 0, 1);
    rtapi_print("[initraj_test]  get ROBOTSAFETYSETTING.LIMIT_ATTITUDE_AUTO: %d", enable_auto);
    std::vector<double> v_cenrpy(3, 0.0);
    ini_file->get("ROBOTSAFETYSETTING.CONICAL_CENRPY", v_cenrpy, {180.000000, 0.000000, 90.000000}, -360, 360);
    rtapi_print("[initraj_test]  get ROBOTSAFETYSETTING.CONICAL_CENRPY: %f,%f,%f ", v_cenrpy[0], v_cenrpy[1], v_cenrpy[2]);
    cenrpy[0] = v_cenrpy[0];
    cenrpy[1] = v_cenrpy[1];
    cenrpy[2] = v_cenrpy[2];
    // //trajInifile->EnableExceptions(ZucIniFile::ERR_CONVERSION);
    // trajInifile->Find(&choice, "CONICAL_TIP_CHOICE", "ROBOTSAFETYSETTING");
    // trajInifile->Find(&tilt, "TOOL_PROPERTIES_TILT", "ROBOTSAFETYSETTING");
    // //rcs_print(" ########################################### initAttitudeSafety  called  tilt = %f\n", tilt);
    // trajInifile->Find(&pan, "TOOL_PROPERTIES_PAN", "ROBOTSAFETYSETTING");
    // //rcs_print(" ########################################### initAttitudeSafety  called  pan = %f\n", pan);
    // trajInifile->Find(&deviation, "DEVIATION", "ROBOTSAFETYSETTING");
    // //rcs_print(" ########################################### initAttitudeSafety  called  deviation = %f\n", deviation);
    // trajInifile->Find(&deviation_warning, "DEVIATION_WARNING", "ROBOTSAFETYSETTING");
    // //rcs_print(" ########################################### initAttitudeSafety  called  deviation = %f\n", deviation);
    // trajInifile->FindArray(cenrpy, 3, "CONICAL_CENRPY", "ROBOTSAFETYSETTING");
    // //rcs_print(" ########################################### initAttitudeSafety  called  cenrpy = %f ,%f ,%f\n", cenrpy[0],cenrpy[1],cenrpy[2]);
    // trajInifile->Find(&enable_open, "LIMIT_ATTITUDE_OPEN", "ROBOTSAFETYSETTING");
    // //rcs_print(" ########################################### initAttitudeSafety  called  cenrpy = %f ,%f ,%f\n", cenrpy[0],cenrpy[1],cenrpy[2]);
    // trajInifile->Find(&enable_auto, "LIMIT_ATTITUDE_AUTO", "ROBOTSAFETYSETTING");
    // //rcs_print(" ########################################### initAttitudeSafety  called  cenrpy = %f ,%f ,%f\n", cenrpy[0],cenrpy[1],cenrpy[2]);

    if (0 != zucSetAttitudeSafety(choice, tilt, pan, deviation, deviation_warning, cenrpy, enable_open, enable_auto))
    {
        return -1;
    }
    return 0;
}
static int initTcpMaxError(Util::IniParser* ini_file)
{
    double max_error;
    // trajInifile->EnableExceptions(ZucIniFile::ERR_CONVERSION);
    // try
    // {
    ini_file->get("ZUC.TCP_POS_MAX_ERROR", max_error, 500.0, 1.0, 1000);
    rtapi_print("[initraj_test]  get ZUC.TCP_POS_MAX_ERROR: %f", max_error);
    // trajInifile->Find(&max_error, "TCP_POS_MAX_ERROR", "ZUC");
    // rcs_print("initTcpMaxError  called  max_error = %f\n", max_error);
    // }
    // catch (ZucIniFile::Exception& e)
    // {
    //     e.Print();
    // }

    if (0 != zucSetTcpMaxError(max_error))
    {
        return -1;
    }
    return 0;
}

static int initPBblendMode(Util::IniParser* ini_file)
{
    auto robot_motion_task = mot::al::MotionProxy::instance().robot(0);
    if (!robot_motion_task.get())
    {
        return -1;
    }
    int pb_blend_mode = 0;
    ini_file->get("ZUC.PB_BLEND_MODE", pb_blend_mode, 0, 0, 1);
    rtapi_print("[initraj_test]  get ZUC.PB_BLEND_MODE: %d", pb_blend_mode);
    //trajInifile->EnableExceptions(ZucIniFile::ERR_CONVERSION);
    // trajInifile->Find(&pb_blend_mode, "PB_BLEND_MODE", "ZUC");
    // rcs_print("initPBblendMode  called  pb_blend_mode = %d\n", pb_blend_mode);
    if (0 != robot_motion_task->zucSetPBblendMode(pb_blend_mode))
    {
        return -1;
    }
    return 0;
}

static int initMovejMovelBlend(Util::IniParser* ini_file)
{
    auto robot_motion_task = mot::al::MotionProxy::instance().robot(0);
    if (!robot_motion_task.get())
    {
        return -1;
    }
    int j_l_enable, j_j_enable = 1;
    // trajInifile->EnableExceptions(ZucIniFile::ERR_CONVERSION);
    ini_file->get("ZUC.MOVEJ_MOVEL_BLEND", j_l_enable, 1, 0, 1);
    rtapi_print("[initraj_test]  get ZUC.MOVEJ_MOVEL_BLEND: %d", j_l_enable);
    ini_file->get("ZUC.MOVEJ_MOVEJ_BLEND", j_j_enable, 1, 0, 1);
    rtapi_print("[initraj_test]  get ZUC.MOVEJ_MOVEJ_BLEND: %d", j_j_enable);
    if (0 != robot_motion_task->zucSetMovejMovelBlend(j_l_enable))
    {
        return -1;
    }
    if (0 != robot_motion_task->zucSetMovejMovejBlend(j_j_enable))
    {
        return -1;
    }

    return 0;
}

static int initCartJogAcc(Util::IniParser* ini_file)
{
    // step jog 的 7 级 加速度
    auto robot_motion_task = mot::al::MotionProxy::instance().robot(0);
    if (!robot_motion_task.get())
    {
        return -1;
    }
    if (ini_file == nullptr)
        return -1;
    double step_jog_pos_acc[7], step_jog_ori_acc[7];

    std::vector<double> v_step_jog_pos_acc(7, 0.0);
    ini_file->get("JOG_ACC.POS_JOG_ACC", v_step_jog_pos_acc, {50.0, 50.0, 10.0, 10.0, 5.0, 5.0, 5.0}, -360, 360);
    rtapi_print("[initraj_test]  get JOG_ACC.POS_JOG_ACC: %f,%f,%f ", v_step_jog_pos_acc[0], v_step_jog_pos_acc[1], v_step_jog_pos_acc[2]);
    std::vector<double> v_step_jog_ori_acc(7, 0.0);
    ini_file->get("JOG_ACC.ORI_JOG_ACC", v_step_jog_ori_acc, {20.0, 20.0, 10.0, 10.0, 5.0, 5.0, 5.0}, -360, 360);
    rtapi_print("[initraj_test]  get JOG_ACC.ORI_JOG_ACC: %f,%f,%f ", v_step_jog_ori_acc[0], v_step_jog_ori_acc[1], v_step_jog_ori_acc[2]);
    for (int i = 0; i < 7; i++)
    {
        step_jog_pos_acc[i] = v_step_jog_pos_acc[i];
        step_jog_ori_acc[i] = v_step_jog_ori_acc[i];
    }

    // trajInifile->FindArray(step_jog_pos_acc, 7, "POS_JOG_ACC", "JOG_ACC");
    // trajInifile->FindArray(step_jog_ori_acc, 7, "ORI_JOG_ACC", "JOG_ACC");

    if (0 != robot_motion_task->zucSetCartJogAcc(step_jog_pos_acc, step_jog_ori_acc))
    {
        return -1;
    }

    return 0;
}

static int initSCBAnalogChannelSetting(Util::IniParser* ini_file)
{
    int ana_chn_0 = 0;
    int ana_chn_1 = 0;
    ini_file->get("IOSETUP.SCB_ANA_CHN_0", ana_chn_0, 1, INT32_MIN, INT32_MAX);
    ini_file->get("IOSETUP.SCB_ANA_CHN_1", ana_chn_1, 1, INT32_MIN, INT32_MAX);
    // trajInifile->Find(&ana_chn_0, "SCB_ANA_CHN_0", "IOSETUP");
    // trajInifile->Find(&ana_chn_1, "SCB_ANA_CHN_1", "IOSETUP");
    if (0 != zucSetupIOChannel(0, 0, ana_chn_0, 0) || 0 != zucSetupIOChannel(0, 1, ana_chn_1, 0))
    {
        return -1;
    }
    return 0;
}

static int initMiniCABDigitalChannelSetting(Util::IniParser* ini_file)
{
    int digital_chn_setting = 0;
    ini_file->get("IOSETUP.MINI_DGT_CHN", digital_chn_setting, 127, INT32_MIN, INT32_MAX);
    // trajInifile->Find(&digital_chn_setting, "MINI_DGT_CHN", "IOSETUP");
    for (int i = 0; i < 7; i++)
    {
        if (0 != zucSetupIOChannel(1, i, IsBitSet(digital_chn_setting, i), 0))
            return -1;
    }

    return 0;
}

static int initMiniCABBRStartVolSetting(Util::IniParser* ini_file)
{
    int br_start_vol = 0;
    ini_file->get("IOSETUP.MINI_BR_START_VOL", br_start_vol, 51, 0, 380);
    //trajInifile->Find(&br_start_vol, "MINI_BR_START_VOL", "IOSETUP");
    if (0 != zucSetBRStartVoltage(br_start_vol, 0))
        return -1;
    return 0;
}

static int initPercentageModeVal(Util::IniParser* ini_file)
{
    double percentage_mode_level1_value = 0.01;
    double percentage_mode_level2_value = 0.01;
    ini_file->get("REDUCEDRATEVAR.REDUCEDRATEL1", percentage_mode_level1_value, 0.01, 0.0, 1.0);
    ini_file->get("REDUCEDRATEVAR.REDUCEDRATEL2", percentage_mode_level2_value, 0.01, 0.0, 1.0);
    ////缩减整改 原缩减模式变为倍率模式，reduce_mode → percentage_mode,为保证兼容usersetting相关不做名称变更
    // trajInifile->Find(&percentage_mode_level1_value, "REDUCEDRATEL1", "REDUCEDRATEVAR");
    // trajInifile->Find(&percentage_mode_level2_value, "REDUCEDRATEL2", "REDUCEDRATEVAR");
    //rcs_print("loaded robot percentage mode level value : %f, %f\n", percentage_mode_level1_value, percentage_mode_level2_value);

    if (0 != zucMotionPercentageModeLevelValue(percentage_mode_level1_value, percentage_mode_level2_value))
    {
        return -1;
    }

    return 0;
}

static int initFreeButtonOpt(Util::IniParser* ini_file)
{
    bool freeButtonOpt = true;
    ini_file->get("FREEBUTTONOPT.FREE_OPT", freeButtonOpt, true);
    //trajInifile->Find(&freeButtonOpt, "FREE_OPT", "FREEBUTTONOPT");
    if (0 != zucMotionFreeButtonOpt(freeButtonOpt, 0))
    {
        return -1;
    }
    return 0;
}

static int initPointButtonOpt(Util::IniParser* ini_file)
{
    bool pointButtonOpt = true;
    ini_file->get("POINTBUTTONOPT.POINT_OPT", pointButtonOpt, true);
    //trajInifile->Find(&pointButtonOpt, "POINT_OPT", "POINTBUTTONOPT");
    if (0 != zucMotionPointButtonOpt(pointButtonOpt, 0))
    {
        return -1;
    }

    return 0;
}

static int initFreeProgButtonOpt(Util::IniParser* ini_file)
{
    bool freeButtonOpt = true;
    bool progButtonOpt = true;
    //trajInifile->EnableExceptions(ZucIniFile::ERR_CONVERSION);
    ini_file->get("FREEPRGOBUTTONOPT.FREE_OPT", freeButtonOpt, true);
    ini_file->get("FREEPRGOBUTTONOPT.FREE_OPT", progButtonOpt, true);

    // trajInifile->Find(&freeButtonOpt, "FREE_OPT", "FREEPRGOBUTTONOPT");
    // trajInifile->Find(&progButtonOpt, "PROG_OPT", "FREEPRGOBUTTONOPT");

    return 0;
}

static int initIdentifyFrictionStartPos(Util::IniParser* ini_file, int joint_num)
{
    int friction_identify_joint_index;
    double friction_identify_start_pos[ZUCMOT_MAX_JOINTS] = {};
    std::vector<double> v_friction_identify_start_pos(ZUCMOT_ROBOT_MAX_JOINTS, 0.0);
    std::vector<double> v_default(ZUCMOT_ROBOT_MAX_JOINTS, 0.0);
    ini_file->get("FRICTION.FRICTION_IDENTIFY_JOINT_INDEX", friction_identify_joint_index, 0, -360, 360);
    for (int i = 1; i <= ZUCMOT_ROBOT_MAX_JOINTS; i++)
    {
        char name[128];
        sprintf(name, "%s_%d", "FRICTION.FRICTION_IDENTIFY_START_POS", i);
        ini_file->get(name, v_friction_identify_start_pos, v_default, -360, 360);
        for (int j = 0; j < ZUCMOT_ROBOT_MAX_JOINTS; j++) { friction_identify_start_pos[j] = v_friction_identify_start_pos[j]; }
        // if (0 != zucSetIdentifyFrictionStartPos(i, friction_identify_start_pos, 0))
        {
            return -1;
        }
    }
    return 0;
}

static int initCollisionSensitivity(Util::IniParser* ini_file)
{
    int joint_sensitivity[ZUCMOT_MAX_JOINTS] = {};
    std::vector<int> v_joint_sensitivity(ZUCMOT_ROBOT_MAX_JOINTS, 0);
    std::vector<int> v_deault(ZUCMOT_ROBOT_MAX_JOINTS, 50);
    ini_file->get("COLLISIONOPTION.JOINT_SENSITIVITY", v_joint_sensitivity, v_deault, 0, 100);
    for (int i = 0; i < ZUCMOT_ROBOT_MAX_JOINTS; i++) { joint_sensitivity[i] = v_joint_sensitivity[i]; }
    // if (0 != zucSetCollisionSensitivity(joint_sensitivity, 0))
    {
        return -1;
    }
    return 0;
}

static int initDragControlEnable(Util::IniParser* ini_file)
{
    int drag_control_enable = 0;
    ini_file->get("DRAGCONTROL.DRAG_CONTROL_ENABLE", (bool&)drag_control_enable, true);

    if (0 != zucSetDragControlEnable(drag_control_enable, 0))
    {
        drag_control_enable = 0;
        return -1;
    }
    return 0;
}

static int initDragControlFcCompensate(Util::IniParser* ini_file)
{
    int drag_control_fc_compensate[ZUCMOT_MAX_JOINTS] = {};

    std::vector<int> v(ZUCMOT_ROBOT_MAX_JOINTS, 0);
    std::vector<int> v_deault(ZUCMOT_ROBOT_MAX_JOINTS, 0);
    ini_file->get("DRAGCONTROL.DRAG_CONTROL_FC_COMPENSATE", v, v_deault, 0, 200);
    for (int i = 0; i < ZUCMOT_ROBOT_MAX_JOINTS; i++) { drag_control_fc_compensate[i] = v[i]; }

    // if (0 != zucSetDragControlFcCompensate(drag_control_fc_compensate, 0))
    // {
    //     return -1;
    // }
    return 0;
}

static int initDragControlFvCompensate(Util::IniParser* ini_file)
{
    int drag_control_fv_compensate[ZUCMOT_ROBOT_MAX_JOINTS] = {};

    std::vector<int> v(ZUCMOT_ROBOT_MAX_JOINTS, 0);
    std::vector<int> v_deault(ZUCMOT_ROBOT_MAX_JOINTS, 0);
    ini_file->get("DRAGCONTROL.DRAG_CONTROL_FV_COMPENSATE", v, v_deault, 0, 200);
    for (int i = 0; i < ZUCMOT_ROBOT_MAX_JOINTS; i++) { drag_control_fv_compensate[i] = v[i]; }

    // if (0 != zucSetDragControlFvCompensate(drag_control_fv_compensate, 0))
    // {
    //     return -1;
    // }
    return 0;
}

static int initDragControlFilterFactor(Util::IniParser* ini_file)
{
    int drag_control_filter_factor[ZUCMOT_ROBOT_MAX_JOINTS] = {};

    std::vector<int> v(ZUCMOT_ROBOT_MAX_JOINTS, 0);
    std::vector<int> v_deault(ZUCMOT_ROBOT_MAX_JOINTS, 0);
    ini_file->get("DRAGCONTROL.DRAG_CONTROL_FILTER_FACTOR", v, v_deault, 0, 200);
    for (int i = 0; i < ZUCMOT_ROBOT_MAX_JOINTS; i++) { drag_control_filter_factor[i] = v[i]; }
    // if (0 != zucSetDragControlFilterFactor(drag_control_filter_factor, 0))
    // {
    //     return -1;
    // }
    return 0;
}

static int initDragControlDitherFactor(Util::IniParser* ini_file)
{
    double drag_control_dither_factor[ZUCMOT_ROBOT_MAX_JOINTS];
    std::vector<double> v(ZUCMOT_ROBOT_MAX_JOINTS, 0);
    std::vector<double> v_deault(ZUCMOT_ROBOT_MAX_JOINTS, 0);
    ini_file->get("DRAGCONTROL.DRAG_CONTROL_FILTER_FACTOR", v, v_deault, 0, 3);
    for (int i = 0; i < ZUCMOT_ROBOT_MAX_JOINTS; i++) { drag_control_dither_factor[i] = v[i]; }

    // if (0 != zucSetDragControlDitherFactor(drag_control_dither_factor, 0))
    // {
    //     return -1;
    // }
    return 0;
}

static int initIdentifyFrictionEndPos(Util::IniParser* ini_file, int joint_num)
{
    int friction_identify_joint_index;
    double friction_identify_end_pos[ZUCMOT_MAX_JOINTS];

    std::vector<double> v_friction_identify_end_pos(ZUCMOT_ROBOT_MAX_JOINTS, 0.0);
    std::vector<double> v_default(ZUCMOT_ROBOT_MAX_JOINTS, 0.0);
    ini_file->get("FRICTION.FRICTION_IDENTIFY_JOINT_INDEX", friction_identify_joint_index, 0, -10000, 10000);
    for (int i = 1; i < ZUCMOT_ROBOT_MAX_JOINTS; i++)
    {
        char name[128];
        sprintf(name, "%s_%d", "FRICTION.FRICTION_IDENTIFY_END_POS", i);
        ini_file->get(name, v_friction_identify_end_pos, v_default, -10000, 10000);
        for (int j = 0; j < ZUCMOT_ROBOT_MAX_JOINTS; j++) { friction_identify_end_pos[j] = v_friction_identify_end_pos[j]; }
        // if (0 != zucSetIdentifyFrictionEndPos(i, friction_identify_end_pos, 0))
        // {
        //     return -1;
        // }
    }
    return 0;
}

static int initCompliantTypeEnable(Util::IniParser* ini_file)
{
    // compliantEnable 此处的compliantEnable表示的时初始化或不初始化
    int compliantEnable = 0;
    ini_file->get("COMPLIANTTYPEENABLE.COMPLIANT_ENABLE", (bool&)compliantEnable, false);
    //trajInifile->EnableExceptions(ZucIniFile::ERR_CONVERSION);
    // trajInifile->Find(&compliantEnable, "COMPLIANT_ENABLE", "COMPLIANTTYPEENABLE");
    int compliantType = 0;
    // 控制器启动时，力控状态一定初始化为0
    if (0 != zucTrajSetComplianceInit(compliantType, compliantEnable, 0))
    {
        return -1;
    }

    return 0;
}

static int initTorqueForwardOption(Util::IniParser* ini_file)
{
    //trajInifile->EnableExceptions(ZucIniFile::ERR_CONVERSION);
    ServoParam servoParam;
    servoParam.paramType = ServoParam::TORQUE_FORWARD_ENA;
    ini_file->get("SERVOPARAM.TORQUE_FORWARD_ENABLE", (bool&)servoParam.paramVal.torque_forward_ena, true);
    //trajInifile->Find(&servoParam.paramVal.torque_forward_ena, "TORQUE_FORWARD_ENABLE", "SERVOPARAM");
    rcs_print("load torque forward %d\n", servoParam.paramVal.torque_forward_ena);

    // if (0 != zucSetServoParam(servoParam, 0))
    // {
    //     return -1;
    // }

    return 0;
}

static int initToolPayLoad(Util::IniParser* ini_file)
{
    Payload payload = {0};
    ini_file->get("TOOLPAYLOAD.PAYLOAD", payload.m, 0, 0, 100);
    std::vector<double> v_p(3, 0);
    ini_file->get("TOOLPAYLOAD.PAYLOADCENTROID", v_p, {0, 0, 0}, -10000, 10000);
    for (int i = 0; i < 3; i++) { payload.p[i] = v_p[i]; }
    // trajInifile->Find(&payload.m, "PAYLOAD", "TOOLPAYLOAD");
    // trajInifile->FindArray(payload.p, 3, "PAYLOADCENTROID", "TOOLPAYLOAD");
    rcs_print("loaded tool payload: %f, %f, %f, %f\n", payload.m, payload.p[0], payload.p[1], payload.p[2]);

    // if (0 != zucToolSetPayload(payload, 0))
    // {
    //     return -1;
    // }

    return 0;
}

int pointRepeat(const CartesianPos* currentPoint, const CartesianPos* point)
{
    int x, y, z, a, b, c;
    x = (currentPoint->point.x - point->point.x) < 1e-3;
    y = (currentPoint->point.y - point->point.y) < 1e-3;
    z = (currentPoint->point.z - point->point.z) < 1e-3;
    a = (currentPoint->rpy.y - point->rpy.y) < 1e-3;
    b = (currentPoint->rpy.p - point->rpy.p) < 1e-3;
    c = (currentPoint->rpy.r - point->rpy.r) < 1e-3;
    if (x && y && z && a && b && c)
    {
        return 1;
    }
    return 0;
}

//将当前点位转换至世界坐标系
void convertPointToWorld(const CartesianPos* user_point, const ZucPose* current_user_offset, CartesianPos* base_point)
{
    if (user_point->point.x <= 1e-6 && user_point->point.y <= 1e-6 && user_point->point.z <= 1e-6 && user_point->rpy.p <= 1e-6 && user_point->rpy.y <= 1e-6 &&
        user_point->rpy.r <= 1e-6)
    {
        return;
    }
    PmRpy user_offset_rpy;
    PmRotationMatrix user_offset_matrix;
    user_offset_rpy.y = current_user_offset->a * PM_PI / 180.0;
    user_offset_rpy.p = current_user_offset->b * PM_PI / 180.0;
    user_offset_rpy.r = current_user_offset->c * PM_PI / 180.0;
    pmRpyMatConvert(&user_offset_rpy, &user_offset_matrix);

    PmRpy user_point_rpy;
    PmRotationMatrix user_point_matrix;
    user_point_rpy.y = user_point->rpy.y * PM_PI / 180.0;
    user_point_rpy.p = user_point->rpy.p * PM_PI / 180.0;
    user_point_rpy.r = user_point->rpy.r * PM_PI / 180.0;
    pmRpyMatConvert(&user_point_rpy, &user_point_matrix);

    PmHomogeneous user_point_homogeneous;
    user_point_homogeneous.rot = user_point_matrix;
    user_point_homogeneous.tran = user_point->point;

    PmHomogeneous offset_homogeneous;
    offset_homogeneous.rot = user_offset_matrix;
    offset_homogeneous.tran = current_user_offset->tran;

    PmHomogeneous result_homogeneous;
    pmHomHomMult(&offset_homogeneous, &user_point_homogeneous, &result_homogeneous);

    PmRpy current_base_rpy;
    pmMatRpyConvert(&result_homogeneous.rot, &current_base_rpy);

    base_point->point = result_homogeneous.tran;
    base_point->rpy.y = current_base_rpy.y * 180.0 / PM_PI;
    base_point->rpy.p = current_base_rpy.p * 180.0 / PM_PI;
    base_point->rpy.r = current_base_rpy.r * 180.0 / PM_PI;
}

//将配置安全平面的点位全部转换为相对于世界坐标系的坐标
int transformSafePlaneRelativeWorld(const ZucPose* currentUserOffset, const SafePlane* currentPlane, SafePlane* plane)
{
    if (NULL == currentUserOffset || NULL == plane)
    {
        return -1;
    }
    if (!pointRepeat(&currentPlane->safePoint, &plane->safePoint))
    {
        convertPointToWorld(&plane->safePoint, currentUserOffset, &plane->safePoint);
    }

    for (int i = 0; i < 3; i++)
    {
        if (!pointRepeat(&currentPlane->planePoints[i], &plane->planePoints[i]))
        {
            convertPointToWorld(&plane->planePoints[i], currentUserOffset, &plane->planePoints[i]);
        }
    }

    return 0;
}
int xmlLoadSafeZone()
{
    SafeZone zone;
    const char* filePath = ZUC_USER_CONFIG_DIR "/safezone.xml";
    int err = SafeZoneConfig::loadSafeZone(filePath, zone);
    if (err)
    {
        //TODO:报警
        return err;
    }
    zucSetSafeZone(zone);
    zucSetSafeZoneEnableOpt(zone.openEnable, zone.autoEnable);
    return 0;
}

int xmlSaveSafeZone(SafeZone zone, int openEnable, int autoEnable, ZucPose currentUserOffset, SafeZone currentSafeZone, int coordinateSystemTransformation)
{
    zone.openEnable = openEnable;
    zone.autoEnable = autoEnable;
    const char* filePath = ZUC_USER_CONFIG_DIR "/safezone.xml";
    if (SafeZoneConfig::saveSafeZone(filePath, zone))
    {
        //TODO:报警
        zucOperatorError(FUNCTION_SAFTY_PLANE_POINT_ERROR, "safety plane point set error: no colinear or coincide is permitted");
        return -1;
    }
    xmlLoadSafeZone();
    return 0;
}

static int initPayloadIdentifyTraj(Util::IniParser* ini_file, int joint_num)
{
    auto initpayloadidentifytraj = mot::al::MotionProxy::instance().robot(0);
    if (!initpayloadidentifytraj.get())
    {
        return -1;
    }
    for (int i = 0; i < 3; i++)
    {
        char startPosEntryName[64];
        char endPosEntryName[64];
        sprintf(startPosEntryName, "PAYLOADIDENTIFICATION.TRAJ_START_%d", i);
        sprintf(endPosEntryName, "PAYLOADIDENTIFICATION.TRAJ_END_%d", i);
        JointTraj jointTraj;
        std::vector<double> v_startJntPos(ZUCMOT_ROBOT_MAX_JOINTS, 0);
        ini_file->get(startPosEntryName, v_startJntPos, {0, 90, 0, 0, 180, 0, 0}, -360, 360);
        for (int i = 0; i < ZUCMOT_ROBOT_MAX_JOINTS; i++) { jointTraj.startJntPos[i] = v_startJntPos[i]; }
        std::vector<double> v_endJntPos(ZUCMOT_ROBOT_MAX_JOINTS, 0);
        ini_file->get(endPosEntryName, v_endJntPos, {0, 90, 0, 0, 180, 0, 0}, -360, 360);
        for (int i = 0; i < ZUCMOT_ROBOT_MAX_JOINTS; i++) { jointTraj.endJntPos[i] = v_endJntPos[i]; }
        initpayloadidentifytraj->zucSetPayloadIdentifyTrajStartPos(i, jointTraj, 0);
        initpayloadidentifytraj->zucSetPayloadIdentifyTrajEndPos(i, jointTraj, 0);
        // rcs_print("initPayloadIdentifyTraj called\n");
    }

    return 0;
}

/*
 loadTraj()

 Loads ini file params for traj

 COORDINATES <char[]>            axes in system
 LINEAR_UNITS <float>            units per mm
 ANGULAR_UNITS <float>           units per degree
 DEFAULT_LINEAR_VELOCITY <float> default linear velocity
 MAX_LINEAR_VELOCITY <float>     max linear velocity
 DEFAULT_LINEAR_ACCEL <float>    default linear acceleration
 MAX_LINEAR_ACCEL <float>        max linear acceleration

 calls:

 zucTrajSetAxes(int axes, int axismask);
 zucTrajSetUnits(double linearUnits, double angularUnits);
 zucTrajSetVelocity(double vel, double ini_maxvel);
 zucTrajSetAcceleration(double acc);
 zucTrajSetMaxVelocity(double vel);
 zucTrajSetMaxAcceleration(double acc);
 */

static int loadTraj(Util::IniParser* ini_file)
{
    auto robot_motion_task = mot::al::MotionProxy::instance().robot(0);
    if (!robot_motion_task.get())
    {
        return -1;
    }
    ZucLinearUnits linearUnits;
    ZucAngularUnits angularUnits;
    double vel;
    double acc;

    //trajInifile->EnableExceptions(ZucIniFile::ERR_CONVERSION);

    int axismask = 0;
    const char* coord = "X Y Z A B C";  //trajInifile->Find("COORDINATES", "TRAJ");
    if (coord)
    {
        if (strchr(coord, 'x') || strchr(coord, 'X'))
        {
            axismask |= 1;
        }
        if (strchr(coord, 'y') || strchr(coord, 'Y'))
        {
            axismask |= 2;
        }
        if (strchr(coord, 'z') || strchr(coord, 'Z'))
        {
            axismask |= 4;
        }
        if (strchr(coord, 'a') || strchr(coord, 'A'))
        {
            axismask |= 8;
        }
        if (strchr(coord, 'b') || strchr(coord, 'B'))
        {
            axismask |= 16;
        }
        if (strchr(coord, 'c') || strchr(coord, 'C'))
        {
            axismask |= 32;
        }
        if (strchr(coord, 'u') || strchr(coord, 'U'))
        {
            axismask |= 64;
        }
        if (strchr(coord, 'v') || strchr(coord, 'V'))
        {
            axismask |= 128;
        }
        if (strchr(coord, 'w') || strchr(coord, 'W'))
        {
            axismask |= 256;
        }
    }
    else
    {
        axismask = 1 | 2 | 4;  // default: XYZ machine
    }
    if (0 != robot_motion_task->zucTrajSetAxes(axismask))
    {
        if (zuc_debug & ZUC_DEBUG_CONFIG)
        {
            rcs_print("bad return value from zucTrajSetAxes\n");
        }
        return -1;
    }

    vel = 1000;  // by default, 1000mm/s
    // and set velocity and max velocity value
    // robot_motion_task->zucTrajSetMaxVelocity(vel);
    // robot_motion_task->zucTrajSetVelocity(0, vel);

    double joint_jerk = 0.0;
    ini_file->get("TRAJ.TRAJ_MAX_JOINT_JERK", joint_jerk, 10000, 250, 100000);
    //trajInifile->Find(&joint_jerk, "TRAJ_MAX_JOINT_JERK", "TRAJ");
    // if (0 != robot_motion_task->zucTrajSetMaxJointJerk(joint_jerk))
    // {
    //     if (zuc_debug & ZUC_DEBUG_CONFIG)
    //     {
    //         rcs_print("bad return value from zucTrajSetMaxJointJerk\n");
    //     }
    //     return -1;
    // }

    double carte_jerk = 0.0;
    ini_file->get("TRAJ.TRAJ_MAX_CARTE_JERK", carte_jerk, 10000, 500, 200000);
    //trajInifile->Find(&carte_jerk, "TRAJ_MAX_CARTE_JERK", "TRAJ");
    // if (0 != robot_motion_task->zucTrajSetMaxCarteJerk(carte_jerk))
    // {
    //     if (zuc_debug & ZUC_DEBUG_CONFIG)
    //     {
    //         rcs_print("bad return value from zucTrajSetMaxCartetJerk\n");
    //     }
    //     return -1;
    // }

    int use_topp = 0;
    ini_file->get("TOPP.USE_TOPP", use_topp, 0);
    //trajInifile->Find(&use_topp, "USE_TOPP", "TOPP");
    if (0 != robot_motion_task->zucTrajGetUseTopp(use_topp))
    {
        rcs_print("bad return value from zucTrajGetUseTopp\n");
        return -1;
    }

    // int add_acc_cons = 0;
    // ini_file->get("TOPP.ADD_ACC_CONS", (bool&)add_acc_cons, false);
    // //trajInifile->Find(&add_acc_cons, "ADD_ACC_CONS", "TOPP");
    // if (0 != zucTrajGetAddAccCons(add_acc_cons))
    // {
    //     rcs_print("bad return value from zucTrajGetAddAccCons\n");
    //     return -1;
    // }

    int filter_size = 0;
    ini_file->get("TOPP.FILTER_SIZE", filter_size, 1);
    if (0 != zucTrajGetFilterSize(filter_size))
    {
        rcs_print("bad return value from zucTrajGetFilterSize\n");
        return -1;
    }

    int add_tor_cons = 1;
    ini_file->get("TOPP.ADD_TOR_CONS", (bool&)add_tor_cons, true);
    //trajInifile->Find(&add_tor_cons, "ADD_TOR_CONS", "TOPP");
    if (0 != zucTrajGetAddTorCons(add_tor_cons))
    {
        rcs_print("bad return value from zucTrajGetAddTorCons\n");
        return -1;
    }
    double tor_portion = 1.0;
    ini_file->get("TOPP.TOR_PORTION", tor_portion, 1.0, 0.000001, 1.0);
    //trajInifile->Find(&tor_portion, "TOR_PORTION", "TOPP");
    if (0 != zucTrajGetTorPortion(tor_portion))
    {
        rcs_print("bad return value from zucTrajGetTorPortion\n");
        return -1;
    }

    double dislen = 0.0;
    ini_file->get("TOPP.DIS_JLEN", tor_portion, 0.05, 0.05, 3.0);
    //trajInifile->Find(&dislen, "DIS_JLEN", "TOPP");
    if (0 != zucTrajSetDisJLen(dislen))
    {
        rcs_print("bad return value from zucTrajSetDisJLen\n");
        return -1;
    }

    dislen = 0.0;
    ini_file->get("TOPP.DIS_LLEN", dislen, 0.5, 0.5, 5.0);
    //trajInifile->Find(&dislen, "DIS_LLEN", "TOPP");
    if (0 != zucTrajSetDisLLen(dislen))
    {
        rcs_print("bad return value from zucTrajSetDisLLen\n");
        return -1;
    }

    dislen = 0.0;
    ini_file->get("TOPP.DIS_CLEN", dislen, 0.5, 0.5, 5.0);
    //trajInifile->Find(&dislen, "DIS_CLEN", "TOPP");
    if (0 != zucTrajSetDisCLen(dislen))
    {
        rcs_print("bad return value from zucTrajSetDisCLen\n");
        return -1;
    }

    double smooth_param = 0.0;
    ini_file->get("TOPP.SMOOTH_PARA", smooth_param, 1, 0.999, 1.0);
    //trajInifile->Find(&smooth_param, "SMOOTH_PARA", "TOPP");
    if (0 != zucTrajSetSmoothPara(smooth_param))
    {
        rcs_print("bad return value from zucTrajSetSmoothPara\n");
        return -1;
    }

    int rate_blend =
        200;  //用于调整速度倍率时降速/提速的离散点个数，rate_blend约大，对速度倍率的修改反应越快，但有概率出现速度跳变的问题，rate_blend越小，反应越慢，但不会有速度跳变的问题
    ini_file->get("TOPP.RATE_BLEND", rate_blend, 200);  //该参数在最新版本已被删除,没有范围
    //trajInifile->Find(&rate_blend, "RATE_BLEND", "TOPP");
    int topp_queue_len = 0;
    ini_file->get("TOPP.MAX_PLANNING_QUEUE_SIZE", topp_queue_len, 80, 30, 200);
    //trajInifile->Find(&topp_queue_len, "MAX_PLANNING_QUEUE_SIZE", "TOPP");
    int max_fw_len = 0;
    ini_file->get("TOPP.MAX_FW_LEN", topp_queue_len, 40, 10, 100);
    //trajInifile->Find(&max_fw_len, "MAX_FW_LEN", "TOPP");
    // if (topp_queue_len < 30)
    // {
    //     topp_queue_len = 30;
    // }
    // if (max_fw_len < 10)
    // {
    //     max_fw_len = 10;
    // }
    rtapi_print(" ********************************** topp_queue_len = %d max_fw_len = %d \n", topp_queue_len, max_fw_len);
    if (0 != robot_motion_task->zucTrajSetRateBlendPara(rate_blend, topp_queue_len, max_fw_len))
    {
        rcs_print("bad return value from zucTrajSetRateBlendPara\n");
        return -1;
    }

    acc = 6000;  // by default, 6000mm/s^2
    // robot_motion_task->zucTrajSetMaxAcceleration(acc);
    // zucTrajSetAcceleration(acc);

    if (zuc_sim == ZUC_SIM_ENA_SOFT)
    {
        robot_motion_task->zucMotionSetRunMode(RUN_MODE_SIM_WITHOUT_CAN);  // simulation with out CAN connection
    }
    else if (zuc_sim == ZUC_SIM_ENA_ON_DEV)
    {
        robot_motion_task->zucMotionSetRunMode(RUN_MODE_SIM_ON_REAL);  // simulation on real device
    }
    else
    {
        int check_can = 1;
        ini_file->get("ZUC.CHECK_CAN_CONNECTION", check_can, 1);
        //trajInifile->Find(&check_can, "CHECK_CAN_CONNECTION", "ZUC");
        if (check_can)
        {
            robot_motion_task->zucMotionSetRunMode(RUN_MODE_REAL);  // real device
        }
        else
        {
            robot_motion_task->zucMotionSetRunMode(RUN_MODE_REAL_NO_CAN_CHECK);  // real device (debug, without CAN check)
        }
    }

    int arcBlendEnable = 1;
    int arcBlendFallbackEnable = 0;
    int arcBlendOptDepth = 50;
    int arcBlendGapCycles = 4;
    double arcBlendRampFreq = 100.0;
    double arcBlendTangentKinkRatio = 0.1;

    // trajInifile->Find(&arcBlendEnable, "ARC_BLEND_ENABLE", "TRAJ");
    // trajInifile->Find(&arcBlendFallbackEnable, "ARC_BLEND_FALLBACK_ENABLE", "TRAJ");
    // trajInifile->Find(&arcBlendOptDepth, "ARC_BLEND_OPTIMIZATION_DEPTH", "TRAJ");
    // trajInifile->Find(&arcBlendGapCycles, "ARC_BLEND_GAP_CYCLES", "TRAJ");
    // trajInifile->Find(&arcBlendRampFreq, "ARC_BLEND_RAMP_FREQ", "TRAJ");
    // trajInifile->Find(&arcBlendTangentKinkRatio, "ARC_BLEND_KINK_RATIO", "TRAJ");

    if (0 != robot_motion_task->zucSetupArcBlends(
                 arcBlendEnable, arcBlendFallbackEnable, arcBlendOptDepth, arcBlendGapCycles, arcBlendRampFreq, arcBlendTangentKinkRatio))
    {
        if (zuc_debug & ZUC_DEBUG_CONFIG)
        {
            rcs_print("bad return value from zucSetupArcBlends\n");
        }
        return -1;
    }

    //TODO update inihal

    double maxFeedScale = 1.0;
    if (0 != zucSetMaxFeedOverride(maxFeedScale))
    {
        if (zuc_debug & ZUC_DEBUG_CONFIG)
        {
            rcs_print("bad return value from zucSetMaxFeedOverride\n");
        }
        return -1;
    }

    return 0;
}

/*
 iniTraj(const char *filename)
 
 Loads ini file parameters for trajectory, from [TRAJ] section
 */
int iniTraj(const char* filename)
{
    if (filename == nullptr)
        return -1;

    Util::IniParser trajInifile(filename);

    // initAutoWork(filename);
    // initOnekeyset(filename);

    // initScriptBackupSettings(filename);

    // initExtIOSetup();

    // initTrajSmooth(filename);

    // if (0 != initRobotTcp(&trajInifile))
    // {
    //     rcs_print("initRobotTcp() failed\n");
    //     return -1;
    // }

    if (0 != initPercentageModeVal(&trajInifile))
    {
        rcs_print("initPercentageModeVal() failed\n");
        return -1;
    }

    if (0 != initFreeButtonOpt(&trajInifile))
    {
        rcs_print("initFreeButtonOpt() failed\n");
        return -1;
    }

    if (0 != initPointButtonOpt(&trajInifile))
    {
        rcs_print("initPointButtonOpt() failed\n");
        return -1;
    }

    if (0 != initFreeProgButtonOpt(&trajInifile))
    {
        rcs_print("initFreeProgButtonOpt() failed\n");
        return -1;
    }

    if (0 != initToolPayLoad(&trajInifile))
    {
        rcs_print("initToolPayLoad() failed\n");
        return -1;
    }

    if (0 != initCompliantTypeEnable(&trajInifile))
    {
        rcs_print("initCompliantTypeEnable() failed\n");
        return -1;
    }

    if (0 != initTorqueForwardOption(&trajInifile))
    {
        rcs_print("initTorqueForwardOption failed\n");
    }

    if (0 != initDragControlEnable(&trajInifile))
    {
        rcs_print("initDragControlEnable() failed\n");
    }

    if (0 != initDragControlFcCompensate(&trajInifile))
    {
        rcs_print("initDragControlFcCompensate() failed\n");
    }

    if (0 != initDragControlFvCompensate(&trajInifile))
    {
        rcs_print("initDragControlFvCompensate() failed\n");
    }

    if (0 != initDragControlFilterFactor(&trajInifile))
    {
        rcs_print("initDragControlFilterFactor() failed\n");
    }

    if (0 != initDragControlDitherFactor(&trajInifile))
    {
        rcs_print("initDragControlDitherFactor() failed\n");
    }

    if (0 != initSetTrackingConfig(&trajInifile))
    {
        rcs_print("initSetTrackingConfig() failed\n");
    }

    if (0 != initDHCom(&trajInifile))
    {
        rcs_print("initDHCom() failed\n");
    }

    if (0 != initLimitLinearVelocity(&trajInifile))
    {
        rcs_print("initLimitLinearVelocity() failed\n");
    }

    if (0 != initLimitJointVelocity(&trajInifile))
    {
        rcs_print("initLimitJointVelocity() failed\n");
    }

    if (0 != initSetPlannnerJerk(&trajInifile))
    {
        rcs_print("initSetPlannnerJerk() failed\n");
    }

    if (0 != initAttitudeSafety(&trajInifile))
    {
        rcs_print("initAttitudeSafety() failed\n");
    }

    if (0 != initTcpMaxError(&trajInifile))
    {
        rcs_print("initTcpMaxError() failed\n");
    }

    //report loadfile error

    if (0 != initPBblendMode(&trajInifile))
    {
        rcs_print("initPBblendMode() failed\n");
        return -1;
    }

    if (0 != initMovejMovelBlend(&trajInifile))
    {
        rcs_print("initMovejMovelBlend() failed\n");
        return -1;
    }

    if (0 != initCartJogAcc(&trajInifile))
    {
        rcs_print("initCartJogAcc() failed\n");
        return -1;
    }

    /*load trajectory values*/
    if (0 != loadTraj(&trajInifile))
    {
        rcs_print("loadTraj() failed\n");
        return -1;
    }

    if (0 != initOperationMode(&trajInifile))
    {
        rcs_print("initOperationMode() failed\n");
    }

    if (zuc_cab_type == 2)
    {
        initSCBAnalogChannelSetting(&trajInifile);
    }
    else if (zuc_cab_type == 3)
    {
        initMiniCABDigitalChannelSetting(&trajInifile);
        initMiniCABBRStartVolSetting(&trajInifile);
    }
    //安全平面配置文件检查，开机时进行
    // xmlSafeZoneProfileCheck();
    // xmlLoadSafeZone();

    // trajInifile.Close();
    return 0;
}

// 以下配置与机器人关节数量相关
int initSetAboutJoint(const char* filename, int joint_num)
{
    if (filename == nullptr)
        return -1;

    Util::IniParser trajInifile(filename);

    if (0 != initIdentifyFrictionEndPos(&trajInifile, joint_num))
    {
        rcs_print("initIdentifyFrictionEndPos() failed\n");
    }

    if (0 != initIdentifyFrictionStartPos(&trajInifile, joint_num))
    {
        rcs_print("initIdentifyFrictionStartPos() failed\n");
    }

    if (0 != initCollisionSensitivity(&trajInifile))
    {
        rcs_print("initCollisionSensitivity() failed\n");
    }

    if (0 != initPayloadIdentifyTraj(&trajInifile, joint_num))
    {
        rcs_print("iniPayloadIndentifyTraj failed\n");
    }
    return 0;
}

int iniSaveAutoWork(int enable, const char* workfile, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        ini_file.set("TASK.DEFAULT_ENABLE", enable);
        ini_file.set("TASK.DEFAULT_PROG", workfile);
        rtapi_print("[initraj_test] TASK.DEFAULT_ENABLE:%d, DEFAULT_PROG:%d\n", enable, workfile);

        // ini_putl("TASK", "DEFAULT_ENABLE", enable, filename);
        // ini_puts("TASK", "DEFAULT_PROG", workfile, filename);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSaveOneKeySet(int autoenable, int autoprogram, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        ini_file.set("TASK.AUTO_ENABLE", autoenable);
        ini_file.set("TASK.AUTO_PROGRAM", autoprogram);

        // ini_putl("TASK", "AUTO_ENABLE", autoenable, filename);
        // ini_putl("TASK", "AUTO_PROGRAM", autoprogram, filename);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSaveRobotName(const char* robotName, const char* filename)
{
    if (filename == NULL || robotName == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        ini_file.set("ROBOT.NAME", robotName);
        rtapi_print("[initraj_test] DEFAULT_PROG:%s\n", robotName);
        //ini_puts("ROBOT", "NAME", robotName, filename);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }
    return 0;
}

int iniSaveIdentifyFrictionEndPos(int friction_identify_joint_index, const double* friction_identify_end_pos, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        ini_file.set("FRICTION.FRICTION_IDENTIFY_JOINT_INDEX", friction_identify_joint_index);
        rtapi_print("[initraj_test] FRICTION.FRICTION_IDENTIFY_JOINT_INDEX:%d\n", friction_identify_joint_index);
        std::vector<double> joint_pos = std::vector<double>(friction_identify_end_pos, friction_identify_end_pos + ZUCMOT_ROBOT_MAX_JOINTS);
        char name[128];
        sprintf(name, "FRICTION.%s_%d", "FRICTION_IDENTIFY_END_POS", friction_identify_joint_index);
        ini_file.set(name, joint_pos);
        std::string msg = "[initraj_test] CUSTOMPOSE.INITIALPOSE:";
        for (int i = 0; i < ZUCMOT_ROBOT_MAX_JOINTS; i++) { msg += std::to_string(joint_pos[i]) + " ,"; }
        msg += "\n";
        rtapi_print(msg.c_str());
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSaveIdentifyFrictionStartPos(int friction_identify_joint_index, const double* friction_identify_start_pos, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        ini_file.set("FRICTION.FRICTION_IDENTIFY_JOINT_INDEX", friction_identify_joint_index);
        rtapi_print("[initraj_test] FRICTION.FRICTION_IDENTIFY_JOINT_INDEX:%d\n", friction_identify_joint_index);

        std::vector<double> input_vector = std::vector<double>(friction_identify_start_pos, friction_identify_start_pos + ZUCMOT_ROBOT_MAX_JOINTS);
        char name[128];
        sprintf(name, "FRICTION.%s_%d", "FRICTION_IDENTIFY_START_POS", friction_identify_joint_index);
        ini_file.set(name, input_vector);
        rtapi_print("[initraj_test] FRICTION.FRICTION_IDENTIFY_START_POS%d:%f, %f, %f, %f, %f, %f\n",
                    friction_identify_joint_index,
                    input_vector[0],
                    input_vector[1],
                    input_vector[2],
                    input_vector[3],
                    input_vector[4],
                    input_vector[5]);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSaveDragControlEnable(int drag_control_enable, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        ini_file.set("DRAGCONTROL.DRAG_CONTROL_ENABLE", drag_control_enable);
        rtapi_print("[initraj_test]DRAGCONTROL.DRAG_CONTROL_ENABLE:%f\n", drag_control_enable);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSaveDragControlFcCompensate(const int* drag_control_fc_compensate, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        std::vector<int> v = std::vector<int>(drag_control_fc_compensate, drag_control_fc_compensate + ZUCMOT_ROBOT_MAX_JOINTS);
        ini_file.set("DRAGCONTROL.DRAG_CONTROL_FC_COMPENSATE", v);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSaveDragControlFvCompensate(const int* drag_control_fv_compensate, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        std::vector<int> v = std::vector<int>(drag_control_fv_compensate, drag_control_fv_compensate + ZUCMOT_ROBOT_MAX_JOINTS);
        ini_file.set("DRAGCONTROL.DRAG_CONTROL_FV_COMPENSATE", v);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSaveDragControlFilterFactor(const int* drag_control_filter_factor, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        std::vector<int> v = std::vector<int>(drag_control_filter_factor, drag_control_filter_factor + ZUCMOT_ROBOT_MAX_JOINTS);
        ini_file.set("DRAGCONTROL.DRAG_CONTROL_FILTER_FACTOR", v);

        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSaveDragControlDitherFactor(const double* drag_control_dither_factor, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        std::vector<int> v = std::vector<int>(drag_control_dither_factor, drag_control_dither_factor + ZUCMOT_ROBOT_MAX_JOINTS);
        ini_file.set("DRAGCONTROL.DRAG_CONTROL_DITHER_FACTOR", v);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSavePayloadIdentifyTrajStartPos(int trajId, JointTraj jointTraj, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        char startPosEntryName[64];
        sprintf(startPosEntryName, "PAYLOADIDENTIFICATION.TRAJ_START_%d", trajId);

        Util::IniParser ini_file(filename);
        std::vector<double> v_input = std::vector<double>(jointTraj.startJntPos, jointTraj.startJntPos + ZUCMOT_ROBOT_MAX_JOINTS);
        ini_file.set(startPosEntryName, v_input);
        std::string msg = "[initraj_test] PAYLOADIDENTIFICATION.TRAJ_START:";
        for (int i = 0; i < ZUCMOT_ROBOT_MAX_JOINTS; i++) { msg += std::to_string(v_input[i]) + " ,"; }
        msg += "\n";
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }
    return 0;
}

int iniSavePayloadIdentifyTrajEndPos(int trajId, JointTraj jointTraj, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        char startPosEntryName[64];
        sprintf(startPosEntryName, "PAYLOADIDENTIFICATION.TRAJ_END_%d", trajId);

        Util::IniParser ini_file(filename);
        std::vector<double> v_input = std::vector<double>(jointTraj.endJntPos, jointTraj.endJntPos + ZUCMOT_ROBOT_MAX_JOINTS);
        ini_file.set(startPosEntryName, v_input);
        std::string msg = "[initraj_test] PAYLOADIDENTIFICATION.TRAJ_END:";
        for (int i = 0; i < ZUCMOT_ROBOT_MAX_JOINTS; i++) { msg += std::to_string(v_input[i]) + " ,"; }
        msg += "\n";
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }
    return 0;
}

int iniSaveSerialNum(int serialNum, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        ini_file.set("ROBOT.SERIAL_NUM", serialNum);
        rtapi_print("[initraj_test] ROBOT.SERIAL_NUM:%d\n", serialNum);

        // ini_putl("ROBOT", "SERIAL_NUM", serialNum, filename);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSaveSlaveConfig(int commType, CommInfo mbComm, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        if (commType == 0)
        {
            Util::IniParser ini_file(filename);
            ini_file.set("MODBUSSLAVE.COMMTYPE", commType);
            rtapi_print("[initraj_test] MODBUSSLAVE.COMMTYPE:%d\n", commType);
            ini_file.set("MODBUSSLAVE.SLAVEID", mbComm.rtuComm.slaveId);
            rtapi_print("[initraj_test] MODBUSSLAVE.SLAVEID:%d\n", mbComm.rtuComm.slaveId);
            ini_file.set("MODBUSSLAVE.BAUDRATE", mbComm.rtuComm.baudrate);
            rtapi_print("[initraj_test] MODBUSSLAVE.BAUDRATE:%d\n", mbComm.rtuComm.baudrate);
            ini_file.set("MODBUSSLAVE.DATABIT", mbComm.rtuComm.databits);
            rtapi_print("[initraj_test] MODBUSSLAVE.DATABIT:%d\n", mbComm.rtuComm.databits);
            ini_file.set("MODBUSSLAVE.STOPBIT", mbComm.rtuComm.stopbits);
            rtapi_print("[initraj_test] MODBUSSLAVE.STOPBIT:%d\n", mbComm.rtuComm.stopbits);
            ini_file.set("MODBUSSLAVE.PARITY", mbComm.rtuComm.parity);
            rtapi_print("[initraj_test] MODBUSSLAVE.PARITY:%d\n", mbComm.rtuComm.parity);

            // ini_putl("MODBUSSLAVE", "COMMTYPE", commType, filename);
            // ini_putl("MODBUSSLAVE", "SLAVEID", mbComm.rtuComm.slaveId, filename);
            // ini_putl("MODBUSSLAVE", "BAUDRATE", mbComm.rtuComm.baudrate, filename);
            // ini_putl("MODBUSSLAVE", "DATABIT", mbComm.rtuComm.databits, filename);
            // ini_putl("MODBUSSLAVE", "STOPBIT", mbComm.rtuComm.stopbits, filename);
            // ini_putl("MODBUSSLAVE", "PARITY", mbComm.rtuComm.parity, filename);
            //syncFileToDisk(filename);
            return 0;
        }
        else if (commType == 1)
        {
            Util::IniParser ini_file(filename);
            ini_file.set("MODBUSSLAVE.COMMTYPE", commType);
            rtapi_print("[initraj_test] MODBUSSLAVE.COMMTYPE:%d\n", commType);
            ini_file.set("MODBUSSLAVE.PORT", mbComm.tcpComm.port);
            rtapi_print("[initraj_test] MODBUSSLAVE.PORT:%d\n", mbComm.tcpComm.port);
            if (strcmp(mbComm.tcpComm.ipaddr, "") != 0)
            {
                std::string ip;
                ini_file.set("MODBUSSLAVE.IPADDR", ip);
                strcpy(mbComm.tcpComm.ipaddr, ip.c_str());
                rtapi_print("[initraj_test] MODBUSSLAVE.IPADDR:%s\n", mbComm.tcpComm.ipaddr);
            }
            // ini_putl("MODBUSSLAVE", "COMMTYPE", commType, filename);
            // ini_putl("MODBUSSLAVE", "PORT", mbComm.tcpComm.port, filename);
            // if (strcmp(mbComm.tcpComm.ipaddr, "") != 0)
            // {
            //     ini_puts("MODBUSSLAVE", "IPADDR", mbComm.tcpComm.ipaddr, filename);
            // }
            //syncFileToDisk(filename);
            return 0;
        }
    }
    catch (...)
    {
        return -1;
    }

    return -1;
}

int iniSaveTorqSensorMode(int mode, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        ini_file.set("TORQSENSOR.ENABLE", mode);
        rtapi_print("[initraj_test] TORQSENSOR.ENABLE:%d\n", mode);

        // ini_putl("TORQSENSOR", "ENABLE", mode, filename);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSaveTorqSensorBrand(int torqsensorBrand, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        ini_file.set("TORQSENSOR.SENSOR_BRAND", torqsensorBrand);
        rtapi_print("[initraj_test] SENSOR_BRAND.ENABLE:%d\n", torqsensorBrand);
        //ini_putl("TORQSENSOR", "SENSOR_BRAND", torqsensorBrand, filename);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSaveTorqSensorComm(int commType, CommInfo commCfg, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        if (commType == 0)
        {
            Util::IniParser ini_file(filename);
            ini_file.set("TORQSENSOR.COMM_TYPE", commType);
            rtapi_print("[initraj_test] TORQSENSOR.SENSOR_IP:%d\n", commType);
            std::string ip;
            ini_file.set("TORQSENSOR.SENSOR_IP", ip);
            strcpy(commCfg.tcpComm.ipaddr, ip.c_str());
            rtapi_print("[initraj_test] TORQSENSOR.SENSOR_BAUDRATE:%s\n", commCfg.tcpComm.ipaddr);
            ini_file.set("TORQSENSOR.SENSOR_PORT", commCfg.tcpComm.port);
            rtapi_print("[initraj_test] TORQSENSOR.SENSOR_DATABIT:%d\n", commCfg.tcpComm.port);

            // ini_putl("TORQSENSOR", "COMM_TYPE", commType, filename);
            // ini_puts("TORQSENSOR", "SENSOR_IP", commCfg.tcpComm.ipaddr, filename);
            // ini_putl("TORQSENSOR", "SENSOR_PORT", commCfg.tcpComm.port, filename);
        }
        else if (commType == 1)
        {
            Util::IniParser ini_file(filename);
            ini_file.set("TORQSENSOR.COMM_TYPE", commType);
            rtapi_print("[initraj_test] TORQSENSOR.COMM_TYPE:%d\n", commType);
            ini_file.set("TORQSENSOR.SENSOR_BAUDRATE", commCfg.rtuComm.baudrate);
            rtapi_print("[initraj_test] TORQSENSOR.SENSOR_BAUDRATE:%d\n", commCfg.rtuComm.baudrate);
            ini_file.set("TORQSENSOR.SENSOR_DATABIT", commCfg.rtuComm.databits);
            rtapi_print("[initraj_test] TORQSENSOR.SENSOR_DATABIT:%d\n", commCfg.rtuComm.databits);
            ini_file.set("TORQSENSOR.SENSOR_STOPBIT", commCfg.rtuComm.stopbits);
            rtapi_print("[initraj_test] TORQSENSOR.SENSOR_STOPBIT:%d\n", commCfg.rtuComm.stopbits);
            ini_file.set("TORQSENSOR.SENSOR_PARITY", commCfg.rtuComm.parity);
            rtapi_print("[initraj_test] TORQSENSOR.SENSOR_PARITY:%d\n", commCfg.rtuComm.parity);

            // ini_putl("TORQSENSOR", "COMM_TYPE", commType, filename);
            // ini_putl("TORQSENSOR", "SENSOR_BAUDRATE", commCfg.rtuComm.baudrate, filename);
            // ini_putl("TORQSENSOR", "SENSOR_DATABIT", commCfg.rtuComm.databits, filename);
            // ini_putl("TORQSENSOR", "SENSOR_STOPBIT", commCfg.rtuComm.stopbits, filename);
            // ini_putl("TORQSENSOR", "SENSOR_PARITY", commCfg.rtuComm.parity, filename);
        }
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSaveTorqSensorPayload(Payload payload, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        std::vector<double> v_input = std::vector<double>(payload.p, payload.p + 3);
        ini_file.set("TORQSENSOR.PAYLOADCENTROID", v_input);
        rtapi_print("[initraj_test] TORQSENSOR.PAYLOADCENTROID:%.3f, %.3f, %.3f\n", v_input[0], v_input[1], v_input[2]);
        ini_file.set("TORQSENSOR.PAYLOAD", payload.m);
        rtapi_print("[initraj_test] TORQSENSOR.PAYLOAD:%d\n", payload.m);

        // char valString[128];
        // sprintf(valString, "{%.3f, %.3f, %.3f}", payload.p[0], payload.p[1], payload.p[2]);
        // ini_puts("TORQSENSOR", "PAYLOADCENTROID", valString, filename);
        // ini_putf("TORQSENSOR", "PAYLOAD", payload.m, filename);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSaveToolPayload(Payload payload, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        std::vector<double> v_input = std::vector<double>(payload.p, payload.p + 3);
        ini_file.set("TOOLPAYLOAD.PAYLOADCENTROID", v_input);
        rtapi_print("[initraj_test] TOOLPAYLOAD.PAYLOADCENTROID:%.3f, %.3f, %.3f\n", v_input[0], v_input[1], v_input[2]);
        ini_file.set("TOOLPAYLOAD.PAYLOAD", payload.m);
        rtapi_print("[initraj_test] TOOLPAYLOAD.PAYLOAD:%d\n", payload.m);

        // char valString[128];
        // sprintf(valString, "{%.3f, %.3f, %.3f}", payload.p[0], payload.p[1], payload.p[2]);
        // ini_puts("TOOLPAYLOAD", "PAYLOADCENTROID", valString, filename);
        // ini_putf("TOOLPAYLOAD", "PAYLOAD", payload.m, filename);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSaveCollisionGmThreshold(const double* gm_threshold, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        std::vector<double> v_input = std::vector<double>(gm_threshold, gm_threshold + 6);
        ini_file.set("COLLISIONOPTION.GM_THRESHOLD", v_input);
        rtapi_print("[initraj_test] COLLISIONOPTION.GM_THRESHOLD:%.3f, %.3f, %.3f,%.3f, %.3f,%.3f\n",
                    v_input[0],
                    v_input[1],
                    v_input[2],
                    v_input[3],
                    v_input[4],
                    v_input[5]);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSaveVelCompliantCtrl(double* velCompliantCtrl, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        std::vector<double> v_input = std::vector<double>(velCompliantCtrl, velCompliantCtrl + 5);
        ini_file.set("VELCOMPLIANTCTRLLEVEL.VELCOMPLIANTCTRL", v_input);
        rtapi_print(
            "[initraj_test] VELCOMPLIANTCTRLLEVEL.VELCOMPLIANTCTRL:%.3f, %.3f, %.3f,%.3f, %.3f\n", v_input[0], v_input[1], v_input[2], v_input[3], v_input[4]);

        // char valString[128];
        // sprintf(
        //     valString, "{%.3f, %.3f, %.3f,%.3f,%.3f}", velCompliantCtrl[0], velCompliantCtrl[1], velCompliantCtrl[2], velCompliantCtrl[3], velCompliantCtrl[4]);
        // ini_puts("VELCOMPLIANTCTRLLEVEL", "VELCOMPLIANTCTRL", valString, filename);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSaveCollisionFilterPara(double gm_filter, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        ini_file.set("COLLISIONOPTION.GM_FILTER", gm_filter);
        rtapi_print("[initraj_test] COLLISIONOPTION.GM_FILTER:%f\n", gm_filter);

        // ini_putf("COLLISIONOPTION", "GM_FILTER", gm_filter, filename);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSaveCompliantTypeEnable(int complinatType, int complinatEnable, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        ini_file.set("COMPLIANTTYPEENABLE.COMPLIANT_TYPE", complinatType);
        ini_file.set("COMPLIANTTYPEENABLE.COMPLIANT_ENABLE", complinatEnable);
        rtapi_print("[initraj_test] COMPLIANTTYPEENABLE.COMPLIANT_TYPE:%f\n", complinatType);
        rtapi_print("[initraj_test] COMPLIANTTYPEENABLE.COMPLIANT_ENABLE:%f\n", complinatEnable);
        // ini_putl("COMPLIANTTYPEENABLE", "COMPLIANT_TYPE", complinatType, filename);
        // ini_putl("COMPLIANTTYPEENABLE", "COMPLIANT_ENABLE", complinatEnable, filename);
        // //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

// @FIXME 适配7轴
int iniSaveTCPCalibration(const unsigned char method,
                          const unsigned char id,
                          const unsigned char saveType,
                          const unsigned char statusFlag,
                          const ZucPose* cPos,
                          const double* jntPos1,
                          const double* jntPos2,
                          const double* jntPos3,
                          const double* jntPos4,
                          const double* jntPos5,
                          const double* jntPos6,
                          const double* tcpError)
{
    if (id < 0 || id >= ZUC_MAX_USER_OFFSET_NUMBER)
        return -1;
    const char* filename = ZUC_USER_CONFIG_DIR "/calibrationsettings.ini";
    char jointKey[16];
    char carteKey[16];
    char tcpSection[24];
    try
    {
        Util::IniParser ini_file(filename);
        std::string node;
        sprintf(tcpSection, "TCPCALIBRATION_%d", id);
        double jointPos[6][ZUCMOT_MAX_JOINTS] = {0.0};
        int m = 0, n = 0;
        if (method == 0)
        {
            n = 4;
            memcpy(jointPos[0], jntPos1, ZUCMOT_MAX_JOINTS * sizeof(double));
            memcpy(jointPos[1], jntPos2, ZUCMOT_MAX_JOINTS * sizeof(double));
            memcpy(jointPos[2], jntPos3, ZUCMOT_MAX_JOINTS * sizeof(double));
            memcpy(jointPos[3], jntPos4, ZUCMOT_MAX_JOINTS * sizeof(double));
            if (saveType == 1)
            {
                // std::string strTCPError =
                //     "{" + std::to_string(tcpError[0]) + std::string(",") + std::to_string(tcpError[1]) + std::string(",") + std::to_string(tcpError[2]) + "}";
                // ini_puts(tcpSection, "TCPERROR", strTCPError.c_str(), filename);
                std::vector<double> v_input;
                v_input.push_back(tcpError[0]);
                v_input.push_back(tcpError[1]);
                v_input.push_back(tcpError[2]);
                node = tcpSection + std::string(".TCPERROR");
                ini_file.set(node, v_input);
            }
        }
        else if (method == 1)
        {
            m = 4, n = 6;
            memcpy(jointPos[4], jntPos5, ZUCMOT_MAX_JOINTS * sizeof(double));
            memcpy(jointPos[5], jntPos6, ZUCMOT_MAX_JOINTS * sizeof(double));
        }
        // ini_putl(tcpSection, "METHOD", method, filename);
        // ini_putl(tcpSection, "STATUSFLAG", statusFlag, filename);
        node = std::string(tcpSection) + ".METHOD";
        ini_file.set(node, method);
        node = std::string(tcpSection) + ".STATUSFLAG";
        ini_file.set(node, statusFlag);

        for (int i = m; i < n; i++)
        {
            std::vector<double> v_CarteList;
            v_CarteList.push_back(cPos[i].tran.x);
            v_CarteList.push_back(cPos[i].tran.y);
            v_CarteList.push_back(cPos[i].tran.z);
            v_CarteList.push_back(cPos[i].a);
            v_CarteList.push_back(cPos[i].b);
            v_CarteList.push_back(cPos[i].c);

            std::vector<double> v_JointList;
            v_JointList.push_back(jointPos[i][0]);
            v_JointList.push_back(jointPos[i][1]);
            v_JointList.push_back(jointPos[i][2]);
            v_JointList.push_back(jointPos[i][3]);
            v_JointList.push_back(jointPos[i][4]);
            v_JointList.push_back(jointPos[i][5]);
            v_JointList.push_back(jointPos[i][6]);
            // strJointList = strJointList.replace(strJointList.end() - 1, strJointList.end(), "}");
            //ini_puts(tcpSection, carteKey, strCarteList.c_str(), filename);
            //ini_puts(tcpSection, jointKey, strJointList.c_str(), filename);
            sprintf(carteKey, "CARTE_%d", i);
            node = std::string(tcpSection) + carteKey;
            ini_file.set(node, v_CarteList);
            sprintf(jointKey, "JOINT_%d", i);
            node = std::string(tcpSection) + jointKey;
            ini_file.set(node, v_JointList);
        }
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSaveUserCalibration(const unsigned char id,
                           const unsigned char statusFlag,
                           const ZucPose cPos1,
                           const ZucPose cPos2,
                           const ZucPose cPos3,
                           const double* jntPos1,
                           const double* jntPos2,
                           const double* jntPos3)
{
    // @FIXME 7轴标定适配
    if (id < 0 || id > 15)
        return -1;
    const char* filename = ZUC_USER_CONFIG_DIR "/calibrationsettings.ini";
    char jointKey[16];
    char carteKey[16];
    char userSection[24];
    try
    {
        Util::IniParser ini_file(filename);
        std::string node;
        sprintf(userSection, "USERCALIBRATION_%d", id);
        double jointPos[3][ZUCMOT_MAX_JOINTS] = {0.0};
        ZucPose cPos[3] = {0.0};
        memcpy(jointPos[0], jntPos1, ZUCMOT_MAX_JOINTS * sizeof(double));
        memcpy(jointPos[1], jntPos2, ZUCMOT_MAX_JOINTS * sizeof(double));
        memcpy(jointPos[2], jntPos3, ZUCMOT_MAX_JOINTS * sizeof(double));
        cPos[0] = cPos1;
        cPos[1] = cPos2;
        cPos[2] = cPos3;
        node = std::string(userSection) + ".STATUSFLAG";
        ini_file.set(node, statusFlag);
        //ini_putl(userSection, "STATUSFLAG", statusFlag, filename);
        for (int i = 0; i < 3; i++)
        {
            // std::string strCarteList("{");
            // strCarteList = strCarteList + std::to_string(cPos[i].tran.x) + std::string(",");
            // strCarteList = strCarteList + std::to_string(cPos[i].tran.y) + std::string(",");
            // strCarteList = strCarteList + std::to_string(cPos[i].tran.z) + std::string(",");
            // strCarteList = strCarteList + std::to_string(cPos[i].a) + std::string(",");
            // strCarteList = strCarteList + std::to_string(cPos[i].b) + std::string(",");
            // strCarteList = strCarteList + std::to_string(cPos[i].c) + std::string(",");
            // strCarteList = strCarteList.replace(strCarteList.end() - 1, strCarteList.end(), "}");

            // std::string strJointList("{");
            // strJointList = strJointList + std::to_string(jointPos[i][0]) + std::string(",");
            // strJointList = strJointList + std::to_string(jointPos[i][1]) + std::string(",");
            // strJointList = strJointList + std::to_string(jointPos[i][2]) + std::string(",");
            // strJointList = strJointList + std::to_string(jointPos[i][3]) + std::string(",");
            // strJointList = strJointList + std::to_string(jointPos[i][4]) + std::string(",");
            // strJointList = strJointList + std::to_string(jointPos[i][5]) + std::string(",");
            // strJointList = strJointList.replace(strJointList.end() - 1, strJointList.end(), "}");
            std::vector<double> v_CarteList;
            v_CarteList.push_back(cPos[i].tran.x);
            v_CarteList.push_back(cPos[i].tran.y);
            v_CarteList.push_back(cPos[i].tran.z);
            v_CarteList.push_back(cPos[i].a);
            v_CarteList.push_back(cPos[i].b);
            v_CarteList.push_back(cPos[i].c);

            std::vector<double> v_JointList;
            v_JointList.push_back(jointPos[i][0]);
            v_JointList.push_back(jointPos[i][1]);
            v_JointList.push_back(jointPos[i][2]);
            v_JointList.push_back(jointPos[i][3]);
            v_JointList.push_back(jointPos[i][4]);
            v_JointList.push_back(jointPos[i][5]);
            v_JointList.push_back(jointPos[i][6]);

            // sprintf(carteKey, "CARTE_%d", i);
            // ini_puts(userSection, carteKey, strCarteList.c_str(), filename);
            // sprintf(jointKey, "JOINT_%d", i);
            // ini_puts(userSection, jointKey, strJointList.c_str(), filename);
            sprintf(carteKey, "CARTE_%d", i);
            node = std::string(userSection) + carteKey;
            ini_file.set(node, v_CarteList);
            sprintf(jointKey, "JOINT_%d", i);
            node = std::string(userSection) + jointKey;
            ini_file.set(node, v_JointList);
        }
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSavePlannerJerk(double joint_jerk, double carte_jerk, const char* filename)
{
    if (filename == NULL)
        return -1;
    // const char* iniMaxJointJerk = "TRAJ_MAX_JOINT_JERK";
    // const char* iniMaxCarteJerk = "TRAJ_MAX_CARTE_JERK";
    try
    {
        Util::IniParser ini_file(filename);
        ini_file.set("TRAJ.TRAJ_MAX_JOINT_JERK", joint_jerk);
        rtapi_print("[initraj_test] TRAJ.TRAJ_MAX_JOINT_JERK:%f\n", joint_jerk);
        ini_file.set("TRAJ.TRAJ_MAX_CARTE_JERK", carte_jerk);
        rtapi_print("[initraj_test] TRAJ.TRAJ_MAX_CARTE_JERK:%f\n", carte_jerk);
        // ini_putl("TRAJ", iniMaxJointJerk, joint_jerk, filename);
        // ini_putl("TRAJ", iniMaxCarteJerk, carte_jerk, filename);

        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }
    return 0;
}

int iniSaveSafeAttitude(PmRpy conicalRpy,
                        double tilt,
                        double pan,
                        double deviation,
                        double deviationwarn,
                        int openEnable,
                        int autoEnable,
                        int tipChoice,
                        const char* filename)
{
    if (filename == NULL)
        return -1;
    // const char* iniTilt = "TOOL_PROPERTIES_TILT";
    // const char* iniPan = "TOOL_PROPERTIES_PAN";
    // const char* iniConiaclCenrpy = "CONICAL_CENRPY";
    // const char* iniDeviation = "DEVIATION";
    // const char* iniDeviationWarning = "DEVIATION_WARNING";
    // const char* iniopenEnable = "LIMIT_ATTITUDE_OPEN";
    // const char* iniautoEnable = "LIMIT_ATTITUDE_AUTO";
    // const char* initipChoice = "CONICAL_TIP_CHOICE";

    // std::string CenrpyList("{");
    try
    {
        Util::IniParser ini_file(filename);
        std::vector<double> v_input;
        v_input.push_back(conicalRpy.y);
        v_input.push_back(conicalRpy.p);
        v_input.push_back(conicalRpy.r);
        ini_file.set("ROBOTSAFETYSETTING.CONICAL_CENRPY", v_input);
        rtapi_print("[initraj_test] ROBOTSAFETYSETTING.CONICAL_CENRPY:%.3f, %.3f, %.3f\n", v_input[0], v_input[1], v_input[2]);

        // CenrpyList = std::string("{");
        // CenrpyList = CenrpyList + std::to_string(conicalRpy.y) + std::string(",");
        // CenrpyList = CenrpyList + std::to_string(conicalRpy.p) + std::string(",");
        // CenrpyList = CenrpyList + std::to_string(conicalRpy.r) + std::string(",");
        // CenrpyList = CenrpyList.replace(CenrpyList.end() - 1, CenrpyList.end(), "}");
        // ini_puts("ROBOTSAFETYSETTING", iniConiaclCenrpy, CenrpyList.c_str(), filename);
        ini_file.set("ROBOTSAFETYSETTING.LIMIT_ATTITUDE_OPEN", openEnable);
        rtapi_print("[initraj_test] ROBOTSAFETYSETTING.LIMIT_ATTITUDE_OPEN:%f\n", openEnable);
        ini_file.set("ROBOTSAFETYSETTING.LIMIT_ATTITUDE_AUTO", autoEnable);
        rtapi_print("[initraj_test] ROBOTSAFETYSETTING.LIMIT_ATTITUDE_AUTO:%f\n", autoEnable);
        ini_file.set("ROBOTSAFETYSETTING.TOOL_PROPERTIES_TILT", tilt);
        rtapi_print("[initraj_test] ROBOTSAFETYSETTING.TOOL_PROPERTIES_TILT:%f\n", tilt);
        ini_file.set("ROBOTSAFETYSETTING.TOOL_PROPERTIES_PAN", pan);
        rtapi_print("[initraj_test] ROBOTSAFETYSETTING.TOOL_PROPERTIES_PAN:%f\n", pan);
        ini_file.set("ROBOTSAFETYSETTING.DEVIATION", deviation);
        rtapi_print("[initraj_test] ROBOTSAFETYSETTING.DEVIATION:%f\n", deviation);
        ini_file.set("ROBOTSAFETYSETTING.DEVIATION_WARNING", deviationwarn);
        rtapi_print("[initraj_test] ROBOTSAFETYSETTING.DEVIATION_WARNING:%f\n", deviationwarn);
        ini_file.set("ROBOTSAFETYSETTING.CONICAL_TIP_CHOICE", tipChoice);
        rtapi_print("[initraj_test] ROBOTSAFETYSETTING.CONICAL_TIP_CHOICE:%f\n", tipChoice);

        // ini_putl("ROBOTSAFETYSETTING", iniopenEnable, openEnable, filename);
        // ini_putl("ROBOTSAFETYSETTING", iniautoEnable, autoEnable, filename);
        // ini_putf("ROBOTSAFETYSETTING", iniTilt, tilt, filename);
        // ini_putf("ROBOTSAFETYSETTING", iniPan, pan, filename);
        // ini_putf("ROBOTSAFETYSETTING", iniDeviation, deviation, filename);
        // ini_putf("ROBOTSAFETYSETTING", iniDeviationWarning, deviationwarn, filename);
        // ini_putl("ROBOTSAFETYSETTING", initipChoice, tipChoice, filename);

        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSaveJointMaxVel(int jointNum, double maxVel, const char* filename)
{
    if (filename == NULL || jointNum < 0 || jointNum >= ZUCMOT_ROBOT_MAX_JOINTS)
    {
        return -1;
    }

    try
    {
        // char sectionName[8];
        // char * sectPrefix = "JOINT_";
        // sprintf(sectionName,"%s%d",sectPrefix,jointNum);
        // ini_putf(sectionName, "JOINT_MAX_VEL", maxVel, filename);
        // //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSaveJointMaxAcc(int jointNum, double maxAcc, const char* filename)
{
    if (filename == NULL || jointNum < 0 || jointNum >= ZUCMOT_ROBOT_MAX_JOINTS)
    {
        return -1;
    }

    try
    {
        char sectionName[32];
        const char* sectPrefix = "JOINT_";
        Util::IniParser ini_file(filename);
        sprintf(sectionName, "%s%d.JOINT_MAX_ACC", sectPrefix, jointNum);
        ini_file.set(sectionName, maxAcc);
        //ini_putf(sectionName, "JOINT_MAX_ACC", maxAcc, filename);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSaveAxisMaxVel(int axisNum, double maxVel, const char* filename)
{
    if (filename == NULL || axisNum < 0 || axisNum >= CART_DIM)
    {
        return -1;
    }
    char axisName[ZUCMOT_MAX_AXIS] = {'X', 'Y', 'Z', 'A', 'B', 'C', 'U', 'V', 'W'};
    try
    {
        char sectionName[32];
        const char* sectPrefix = "AXIS_";
        sprintf(sectionName, "%s%c.AXIS_MAX_VEL", sectPrefix, axisName[axisNum]);
        Util::IniParser ini_file(filename);
        ini_file.set(sectionName, maxVel);
        //ini_putf(sectionName, "AXIS_MAX_VEL", maxVel, filename);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }
    return 0;
}

int iniSaveAxisMaxAcc(int axisNum, double maxAcc, const char* filename)
{
    if (filename == NULL || axisNum < 0 || axisNum >= CART_DIM)
    {
        return -1;
    }
    char axisName[ZUCMOT_MAX_AXIS] = {'X', 'Y', 'Z', 'A', 'B', 'C', 'U', 'V', 'W'};
    try
    {
        char sectionName[64];
        const char* sectPrefix = "AXIS_";
        sprintf(sectionName, "%s%c.AXIS_MAX_ACC", sectPrefix, axisName[axisNum]);
        Util::IniParser ini_file(filename);
        ini_file.set(sectionName, maxAcc);
        //ini_putf(sectionName, "AXIS_MAX_ACC", maxAcc, filename);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }
    return 0;
}

int iniSaveDHParam(int serialNum, DHParam dhParam, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        char strSNKey[64];
        sprintf(strSNKey, "DHPARAM.%09d", serialNum);
        // std::string strItemList("{");
        std::vector<double> v_input;
        if (isZuCProCobot(serialNum))
        {  // Zu Series
            v_input.clear();
            v_input.push_back(dhParam.d[0]);
            v_input.push_back(dhParam.d[3]);
            v_input.push_back(dhParam.d[4]);
            v_input.push_back(dhParam.d[5]);
            v_input.push_back(dhParam.a[2]);
            v_input.push_back(dhParam.a[3]);
            for (int i = 0; i < 6; ++i) { v_input.push_back(dhParam.joint_homeoff[i]); }
        }
        else if (isMiniCobot(serialNum))
        {
            v_input.clear();
            for (int i = 0; i < 6; i++) { v_input.push_back(dhParam.d[i]); }
            for (int i = 0; i < 6; i++) { v_input.push_back(dhParam.a[i]); }
            for (int i = 0; i < 6; i++) { v_input.push_back(dhParam.joint_homeoff[i]); }
        }
        else if (isMiniCobot(serialNum))
        {
            for (int i = 0; i < 7; i++) { v_input.push_back(dhParam.d[i]); }
            for (int i = 0; i < 7; i++) { v_input.push_back(dhParam.a[i]); }
            for (int i = 0; i < 7; i++) { v_input.push_back(dhParam.joint_homeoff[i]); }
        }
        ini_file.set(strSNKey, v_input);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSavePayload(Payload payload, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        ini_file.set("WORKPAYLOAD.PAYLOAD", payload.m);
        ini_file.set("WORKPAYLOAD.XPOFFSET", payload.p[0]);
        ini_file.set("WORKPAYLOAD.YPOFFSET", payload.p[1]);
        ini_file.set("WORKPAYLOAD.ZPOFFSET", payload.p[2]);
        // ini_putf("WORKPAYLOAD", "PAYLOAD", payload.m, filename);
        // ini_putf("WORKPAYLOAD", "XPOFFSET", payload.p[0], filename);
        // ini_putf("WORKPAYLOAD", "YPOFFSET", payload.p[1], filename);
        // ini_putf("WORKPAYLOAD", "ZPOFFSET", payload.p[2], filename);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSavePercentageModeValue(double percentage_mode_level1_val, double percentage_mode_level2_val, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        ////缩减整改 原缩减模式变为倍率模式，reduce_mode → percentage_mode,为保证兼容usersetting相关不做名称变更
        Util::IniParser ini_file(filename);
        ini_file.set("REDUCEDRATEVAR.REDUCEDRATEL1", percentage_mode_level1_val);
        ini_file.set("REDUCEDRATEVAR.REDUCEDRATEL2", percentage_mode_level2_val);
        // ini_putf("REDUCEDRATEVAR", "REDUCEDRATEL1", percentage_mode_level1_val, filename);
        // ini_putf("REDUCEDRATEVAR", "REDUCEDRATEL2", percentage_mode_level2_val, filename);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSavefreeButtonOpt(bool freeButtonOpt, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        ini_file.set("FREEBUTTONOPT.FREE_OPT", freeButtonOpt);
        //ini_putf("FREEBUTTONOPT", "FREE_OPT", freeButtonOpt, filename);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSavefreeProgButtonOpt(bool freeButtonOpt, bool progButtonOpt, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        ini_file.set("FREEPRGOBUTTONOPT.FREE_OPT", freeButtonOpt);
        ini_file.set("FREEPRGOBUTTONOPT.PROG_OPT", progButtonOpt);
        // ini_putl("FREEPRGOBUTTONOPT", "FREE_OPT", freeButtonOpt, filename);
        // ini_putl("FREEPRGOBUTTONOPT", "PROG_OPT", progButtonOpt, filename);

        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSavePointButtonOpt(bool pointButtonOpt, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        ini_file.set("POINTBUTTONOPT.POINT_OPT", pointButtonOpt);
        // ini_putf("POINTBUTTONOPT", "POINT_OPT", pointButtonOpt, filename);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSaveTorqueForwardOpt(int torqueForwardEna, const char* filename)
{
    if (filename == NULL)
        return -1;
    try
    {
        Util::IniParser ini_file(filename);
        ini_file.set("SERVOPARAM.TORQUE_FORWARD_ENABLE", torqueForwardEna);
        // ini_putl("SERVOPARAM", "TORQUE_FORWARD_ENABLE", torqueForwardEna, filename);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }
    return 0;
}

int iniSaveAnalogChannelSetting(int index, int anaChnSetting, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        if (index == 0)
        {
            ini_file.set("IOSETUP.SCB_ANA_CHN_0", anaChnSetting);
            // ini_putl("IOSETUP", "SCB_ANA_CHN_0", anaChnSetting, filename);
        }
        else if (index == 1)
        {
            ini_file.set("IOSETUP.SCB_ANA_CHN_1", anaChnSetting);
            // ini_putl("IOSETUP", "SCB_ANA_CHN_1", anaChnSetting, filename);
        }
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSaveDigtalChannelSetting(int index, int dgtChnSetting, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        int chnSettings = 0;

        ini_file.get("IOSETUP.MINI_DGT_CHN", chnSettings, 103, 96, 127);
        //int chnSettings = ini_getl("IOSETUP", "MINI_DGT_CHN", 0, filename);
        if (index < 0 || index >= 7)
            return -1;
        if (dgtChnSetting)
            SetBit(chnSettings, index);
        else
            ClearBit(chnSettings, index);
        ini_file.set("IOSETUP.MINI_DGT_CHN", chnSettings);
        // ini_putl("IOSETUP", "MINI_DGT_CHN", chnSettings, filename);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSaveBRStartVoltage(int startVoltage, const char* filename)
{
    if (filename == NULL)
        return -1;
    try
    {
        Util::IniParser ini_file(filename);
        ini_file.set("IOSETUP.MINI_BR_START_VOL", startVoltage);
        //ini_putl("IOSETUP", "MINI_BR_START_VOL", startVoltage, filename);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }
    return 0;
}

int iniSaveServoMoveFilterType(int filterType, const char* filename)
{
    if (filename == NULL)
        return -1;
    try
    {
        Util::IniParser ini_file(filename);
        ini_file.set("SERVOMOVE.FILTER_TYPE", filterType);
        //ini_putl("SERVOMOVE", "FILTER_TYPE", filterType, filename);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }
    return 0;
}

int iniSaveInstallationAngle(double* appang, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        ini_file.set("BASEOFFSET.APPANGX", appang[0]);
        ini_file.set("BASEOFFSET.APPANGY", appang[1]);
        ini_file.set("BASEOFFSET.APPANGZ", appang[2]);
        // ini_putf("BASEOFFSET", "APPANGX", appang[0], filename);
        // ini_putf("BASEOFFSET", "APPANGY", appang[1], filename);
        // ini_putf("BASEOFFSET", "APPANGZ", appang[2], filename);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }
    return 0;
}

int iniSaveScriptBackupSetting(int enable, double interval, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        ini_file.set("SCRIPTAUTOBACKUP.ENABLE", enable);
        ini_file.set("SCRIPTAUTOBACKUP.INTERVAL", interval);
        // ini_putl("SCRIPTAUTOBACKUP", "ENABLE", enable, filename);
        // ini_putf("SCRIPTAUTOBACKUP", "INTERVAL", interval, filename);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }
    return 0;
}

int iniSaveFuncDIMap(FuncIOMap funcIOMap, const char* filename)
{
    if (filename == NULL)
        return -1;

    int ret = 0;
    Util::IniParser ini_file(filename);
    try
    {
        std::string strOptList("{");
        strOptList += std::to_string(funcIOMap.task_run_di.type) + std::string(":") + std::to_string(funcIOMap.task_run_di.index) + std::string("}");
        ret = ini_file.set("FUNCTIONIO.TASK_RUN_MAP_DI", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "TASK_RUN_MAP_DI", strOptList.c_str(), filename) && ret;
        // rcs_print("data write to  TASK_RUN_MAP_DI: %s, ret=%d\n", strOptList.c_str(), ret);

        strOptList = std::string("{");
        strOptList += std::to_string(funcIOMap.task_pause_di.type) + std::string(":") + std::to_string(funcIOMap.task_pause_di.index) + std::string("}");
        ret = ini_file.set("FUNCTIONIO.TASK_PAUSE_MAP_DI", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "TASK_PAUSE_MAP_DI", strOptList.c_str(), filename) && ret;
        // rcs_print("data write to  TASK_PAUSE_MAP_DI: %s, ret=%d\n", strOptList.c_str(), ret);

        strOptList = std::string("{");
        strOptList += std::to_string(funcIOMap.task_resume_di.type) + std::string(":") + std::to_string(funcIOMap.task_resume_di.index) + std::string("}");
        ret = ini_file.set("FUNCTIONIO.TASK_RESUME_MAP_DI", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "TASK_RESUME_MAP_DI", strOptList.c_str(), filename) && ret;
        // rcs_print("data write to  TASK_RESUME_MAP_DI: %s, ret=%d\n", strOptList.c_str(), ret);

        strOptList = std::string("{");
        strOptList += std::to_string(funcIOMap.task_stop_di.type) + std::string(":") + std::to_string(funcIOMap.task_stop_di.index) + std::string("}");
        ret = ini_file.set("FUNCTIONIO.TASK_STOP_MAP_DI", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "TASK_STOP_MAP_DI", strOptList.c_str(), filename) && ret;
        // rcs_print("data write to  TASK_STOP_MAP_DI: %s, ret=%d\n", strOptList.c_str(), ret);

        strOptList = std::string("{");
        strOptList += std::to_string(funcIOMap.power_on_di.type) + std::string(":") + std::to_string(funcIOMap.power_on_di.index) + std::string("}");
        ret = ini_file.set("FUNCTIONIO.POWER_ON_MAP_DI", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "POWER_ON_MAP_DI", strOptList.c_str(), filename) && ret;
        // rcs_print("data write to  POWER_ON_MAP_DI: %s, ret=%d\n", strOptList.c_str(), ret);

        strOptList = std::string("{");
        strOptList += std::to_string(funcIOMap.power_off_di.type) + std::string(":") + std::to_string(funcIOMap.power_off_di.index) + std::string("}");
        ret = ini_file.set("FUNCTIONIO.POWER_OFF_MAP_DI", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "POWER_OFF_MAP_DI", strOptList.c_str(), filename) && ret;
        // rcs_print("data write to  POWER_OFF_MAP_DI: %s, ret=%d\n", strOptList.c_str(), ret);

        strOptList = std::string("{");
        strOptList += std::to_string(funcIOMap.servo_enable_di.type) + std::string(":") + std::to_string(funcIOMap.servo_enable_di.index) + std::string("}");
        ret = ini_file.set("FUNCTIONIO.SERVO_ENABLE_MAP_DI", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "SERVO_ENABLE_MAP_DI", strOptList.c_str(), filename) && ret;
        // rcs_print("data write to  SERVO_ENABLE_MAP_DI: %s, ret=%d\n", strOptList.c_str(), ret);

        strOptList = std::string("{");
        strOptList += std::to_string(funcIOMap.servo_disable_di.type) + std::string(":") + std::to_string(funcIOMap.servo_disable_di.index) + std::string("}");
        ret = ini_file.set("FUNCTIONIO.SERVO_DISABLE_MAP_DI", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "SERVO_DISABLE_MAP_DI", strOptList.c_str(), filename) && ret;
        // rcs_print("data write to  SERVO_DISABLE_MAP_DI: %s, ret=%d\n", strOptList.c_str(), ret);

        ////缩减整改 原缩减模式变为倍率模式，reduce_mode → percentage_mode,为保证兼容usersetting相关不做名称变更
        strOptList = std::string("{");
        strOptList += std::to_string(funcIOMap.percentage_mode_level1_di.type) + std::string(":") + std::to_string(funcIOMap.percentage_mode_level1_di.index) +
                      std::string("}");
        ret = ini_file.set("FUNCTIONIO.REDUCED_MODE_MAP_L1_DI", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "REDUCED_MODE_MAP_L1_DI", strOptList.c_str(), filename) && ret;
        // rcs_print("data write to  PERCENTAGE_MODE_MAP_LEVEL1_DI: %s, ret=%d\n", strOptList.c_str(), ret);

        strOptList = std::string("{");
        strOptList += std::to_string(funcIOMap.stop_mode_di.type) + std::string(":") + std::to_string(funcIOMap.stop_mode_di.index) + std::string("}");
        ret = ini_file.set("FUNCTIONIO.STOP_MODE_MAP_DI", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "STOP_MODE_MAP_DI", strOptList.c_str(), filename) && ret;
        // rcs_print("data write to  STOP_MODE_MAP_DI: %s, ret=%d\n", strOptList.c_str(), ret);

        strOptList = std::string("{");
        strOptList +=
            std::to_string(funcIOMap.back_to_inipos_di.type) + std::string(":") + std::to_string(funcIOMap.back_to_inipos_di.index) + std::string("}");
        ret = ini_file.set("FUNCTIONIO.BACK_TO_INIPOS_MAP_DI", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "BACK_TO_INIPOS_MAP_DI", strOptList.c_str(), filename) && ret;
        // rcs_print("data write to  STOP_MODE_MAP_DI: %s, ret=%d\n", strOptList.c_str(), ret);

        //缩减整改 原缩减模式变为倍率模式，reduce_mode → percentage_mode,为保证兼容usersetting相关不做名称变更
        strOptList = std::string("{");
        strOptList += std::to_string(funcIOMap.percentage_mode_level2_di.type) + std::string(":") + std::to_string(funcIOMap.percentage_mode_level2_di.index) +
                      std::string("}");
        ret = ini_file.set("FUNCTIONIO.REDUCED_MODE_MAP_L2_DI", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "REDUCED_MODE_MAP_L2_DI", strOptList.c_str(), filename) && ret;
        // rcs_print("data write to  PERCENTAGE_MODE_MAP_LEVEL2_DI: %s, ret=%d\n", strOptList.c_str(), ret);

        strOptList = std::string("{");
        strOptList += std::to_string(funcIOMap.clear_error_di.type) + std::string(":") + std::to_string(funcIOMap.clear_error_di.index) + std::string("}");
        ret = ini_file.set("FUNCTIONIO.CLEAR_ERROR_DI", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "CLEAR_ERROR_DI", strOptList.c_str(), filename) && ret;

        strOptList = std::string("{");
        strOptList += std::to_string(funcIOMap.drag_mode_di.type) + std::string(":") + std::to_string(funcIOMap.drag_mode_di.index) + std::string("}");
        ret = ini_file.set("FUNCTIONIO.DRAG_MODE_ENABLE_DI", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "DRAG_MODE_ENABLE_DI", strOptList.c_str(), filename) && ret;

        strOptList = std::string("{");
        strOptList +=
            std::to_string(funcIOMap.drag_mode_disable_di.type) + std::string(":") + std::to_string(funcIOMap.drag_mode_disable_di.index) + std::string("}");
        ret = ini_file.set("FUNCTIONIO.DRAG_MODE_DISABLE_DI", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "DRAG_MODE_DISABLE_DI", strOptList.c_str(), filename) && ret;

        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return ret ? 0 : -1;
}

int iniSaveFuncDOMap(FuncIOMap funcIOMap, const char* filename)
{
    if (filename == NULL)
        return -1;

    int ret = 0;

    try
    {
        Util::IniParser ini_file(filename);
        std::string strOptList("{");
        /*task idle status dout map*/
        strOptList = std::string("{");
        for (int i = 0; i < 4; ++i)
        {
            strOptList = strOptList + std::to_string(funcIOMap.task_idle_do[i].type) + std::string(":") + std::to_string(funcIOMap.task_idle_do[i].index) +
                         std::string(",");
        }
        strOptList = strOptList.replace(strOptList.end() - 1, strOptList.end(), "}");
        ret = ini_file.set("FUNCTIONIO.TASK_IDLE_DO", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "TASK_IDLE_DO", strOptList.c_str(), filename) && ret;
        // rcs_print("data write to  TASK_IDLE_DO: %s, ret=%d\n", strOptList.c_str(), ret);

        /*task paused status dout map*/
        strOptList = std::string("{");
        for (int i = 0; i < 4; ++i)
        {
            strOptList = strOptList + std::to_string(funcIOMap.task_paused_do[i].type) + std::string(":") + std::to_string(funcIOMap.task_paused_do[i].index) +
                         std::string(",");
        }
        strOptList = strOptList.replace(strOptList.end() - 1, strOptList.end(), "}");
        ret = ini_file.set("FUNCTIONIO.TASK_PAUSED_DO", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "TASK_PAUSED_DO", strOptList.c_str(), filename) && ret;
        // rcs_print("data write to  TASK_PAUSED_DO: %s, ret=%d\n", strOptList.c_str(), ret);

        /*task running status dout map*/
        strOptList = std::string("{");
        for (int i = 0; i < 4; ++i)
        {
            strOptList = strOptList + std::to_string(funcIOMap.task_running_do[i].type) + std::string(":") +
                         std::to_string(funcIOMap.task_running_do[i].index) + std::string(",");
        }
        strOptList = strOptList.replace(strOptList.end() - 1, strOptList.end(), "}");
        ret = ini_file.set("FUNCTIONIO.TASK_RUNNING_DO", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "TASK_RUNNING_DO", strOptList.c_str(), filename) && ret;
        // rcs_print("data write to  TASK_RUNNING_DO: %s, ret=%d\n", strOptList.c_str(), ret);

        /*fault status dout map*/
        strOptList = std::string("{");
        for (int i = 0; i < 4; ++i)
        {
            strOptList = strOptList + std::to_string(funcIOMap.stat_fault_do[i].type) + std::string(":") + std::to_string(funcIOMap.stat_fault_do[i].index) +
                         std::string(",");
        }
        strOptList = strOptList.replace(strOptList.end() - 1, strOptList.end(), "}");
        ret = ini_file.set("FUNCTIONIO.STAT_FAULT_DO", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "STAT_FAULT_DO", strOptList.c_str(), filename) && ret;
        // rcs_print("data write to  STAT_FAULT_DO: %s, ret=%d\n", strOptList.c_str(), ret);

        /*poweron status dout map*/
        strOptList = std::string("{");
        for (int i = 0; i < 4; ++i)
        {
            strOptList = strOptList + std::to_string(funcIOMap.stat_poweron_do[i].type) + std::string(":") +
                         std::to_string(funcIOMap.stat_poweron_do[i].index) + std::string(",");
        }
        strOptList = strOptList.replace(strOptList.end() - 1, strOptList.end(), "}");
        ret = ini_file.set("FUNCTIONIO.STAT_POWERON_DO", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "STAT_POWERON_DO", strOptList.c_str(), filename) && ret;
        // rcs_print("data write to  STAT_POWERON_DO: %s, ret=%d\n", strOptList.c_str(), ret);

        /*enabled status dout map*/
        strOptList = std::string("{");
        for (int i = 0; i < 4; ++i)
        {
            strOptList = strOptList + std::to_string(funcIOMap.stat_enabled_do[i].type) + std::string(":") +
                         std::to_string(funcIOMap.stat_enabled_do[i].index) + std::string(",");
        }
        strOptList = strOptList.replace(strOptList.end() - 1, strOptList.end(), "}");
        ret = ini_file.set("FUNCTIONIO.STAT_ENABLED_DO", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "STAT_ENABLED_DO", strOptList.c_str(), filename) && ret;

        // rcs_print("data write to  STAT_ENABLED_DO: %s, ret=%d\n", strOptList.c_str(), ret);
        /*motion status dout map*/
        strOptList = std::string("{");
        for (int i = 0; i < 4; ++i)
        {
            strOptList = strOptList + std::to_string(funcIOMap.stat_motion_do[i].type) + std::string(":") + std::to_string(funcIOMap.stat_motion_do[i].index) +
                         std::string(",");
        }
        strOptList = strOptList.replace(strOptList.end() - 1, strOptList.end(), "}");
        ret = ini_file.set("FUNCTIONIO.STAT_MOTION_DO", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "STAT_MOTION_DO", strOptList.c_str(), filename) && ret;
        // rcs_print("data write to  STAT_MOTION_DO: %s, ret=%d\n", strOptList.c_str(), ret);

        /*static status dout map*/
        strOptList = std::string("{");
        for (int i = 0; i < 4; ++i)
        {
            strOptList = strOptList + std::to_string(funcIOMap.stat_static_do[i].type) + std::string(":") + std::to_string(funcIOMap.stat_static_do[i].index) +
                         std::string(",");
        }
        strOptList = strOptList.replace(strOptList.end() - 1, strOptList.end(), "}");
        ret = ini_file.set("FUNCTIONIO.STAT_STATIC_DO", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "STAT_STATIC_DO", strOptList.c_str(), filename) && ret;
        // rcs_print("data write to  STAT_STATIC_DO: %s, ret=%d\n", strOptList.c_str(), ret);

        /*boot status dout map*/
        strOptList = std::string("{");
        for (int i = 0; i < 4; ++i)
        {
            strOptList = strOptList + std::to_string(funcIOMap.stat_boot_do[i].type) + std::string(":") + std::to_string(funcIOMap.stat_boot_do[i].index) +
                         std::string(",");
        }
        strOptList = strOptList.replace(strOptList.end() - 1, strOptList.end(), "}");
        ret = ini_file.set("FUNCTIONIO.STAT_BOOT_DO", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "STAT_BOOT_DO", strOptList.c_str(), filename) && ret;
        // rcs_print("data write to  STAT_BOOT_DO: %s, ret=%d\n", strOptList.c_str(), ret);

        /*emergency stop status dout map*/
        strOptList = std::string("{");
        for (int i = 0; i < 4; ++i)
        {
            strOptList = strOptList + std::to_string(funcIOMap.stat_emergency_stop_do[i].type) + std::string(":") +
                         std::to_string(funcIOMap.stat_emergency_stop_do[i].index) + std::string(",");
        }
        strOptList = strOptList.replace(strOptList.end() - 1, strOptList.end(), "}");
        ret = ini_file.set("FUNCTIONIO.STAT_EMERGEMCY_STOP_DO", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "STAT_EMERGEMCY_STOP_DO", strOptList.c_str(), filename) && ret;

        /*percentage mode 2 status dout map*/
        strOptList = std::string("{");
        for (int i = 0; i < 4; ++i)
        {
            strOptList = strOptList + std::to_string(funcIOMap.stat_percentage_mode_level2_do[i].type) + std::string(":") +
                         std::to_string(funcIOMap.stat_percentage_mode_level2_do[i].index) + std::string(",");
        }
        strOptList = strOptList.replace(strOptList.end() - 1, strOptList.end(), "}");
        //缩减整改 原缩减模式变为倍率模式，reduce_mode → percentage_mode,为保证兼容usersetting相关不做名称变更
        ret = ini_file.set("FUNCTIONIO.STAT_REDUCED_MODE_DO", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "STAT_REDUCED_MODE_DO", strOptList.c_str(), filename) && ret;

        /*system protective stop status dout map*/
        strOptList = std::string("{");
        for (int i = 0; i < 4; ++i)
        {
            strOptList = strOptList + std::to_string(funcIOMap.stat_sys_protective_stop_do[i].type) + std::string(":") +
                         std::to_string(funcIOMap.stat_sys_protective_stop_do[i].index) + std::string(",");
        }
        strOptList = strOptList.replace(strOptList.end() - 1, strOptList.end(), "}");
        ret = ini_file.set("FUNCTIONIO.STAT_SYS_PROTECTIVE_STOP_DO", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "STAT_SYS_PROTECTIVE_STOP_DO", strOptList.c_str(), filename) && ret;

        /*initial pose status dout map*/
        strOptList = std::string("{");
        for (int i = 0; i < 4; ++i)
        {
            strOptList = strOptList + std::to_string(funcIOMap.stat_initial_pose_do[i].type) + std::string(":") +
                         std::to_string(funcIOMap.stat_initial_pose_do[i].index) + std::string(",");
        }
        strOptList = strOptList.replace(strOptList.end() - 1, strOptList.end(), "}");
        ret = ini_file.set("FUNCTIONIO.STAT_INITIAL_POSE_DO", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "STAT_INITIAL_POSE_DO", strOptList.c_str(), filename) && ret;

        /*percentage mode 1 status dout map*/
        strOptList = std::string("{");
        for (int i = 0; i < 4; ++i)
        {
            strOptList = strOptList + std::to_string(funcIOMap.stat_percentage_mode_level1_do[i].type) + std::string(":") +
                         std::to_string(funcIOMap.stat_percentage_mode_level1_do[i].index) + std::string(",");
        }
        strOptList = strOptList.replace(strOptList.end() - 1, strOptList.end(), "}");
        ret = ini_file.set("FUNCTIONIO.STAT_PERCENTAGE_MODE_LEVEL1_DO", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "STAT_PERCENTAGE_MODE_LEVEL1_DO", strOptList.c_str(), filename) && ret;

        /*drag mode status dout map*/
        strOptList = std::string("{");
        for (int i = 0; i < 4; ++i)
        {
            strOptList = strOptList + std::to_string(funcIOMap.stat_drag_mode_do[i].type) + std::string(":") +
                         std::to_string(funcIOMap.stat_drag_mode_do[i].index) + std::string(",");
        }
        strOptList = strOptList.replace(strOptList.end() - 1, strOptList.end(), "}");
        ret = ini_file.set("FUNCTIONIO.STAT_DRAG_MODE_DO", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "STAT_DRAG_MODE_DO", strOptList.c_str(), filename) && ret;

        /*collision mode status dout map*/
        strOptList = std::string("{");
        for (int i = 0; i < 4; ++i)
        {
            strOptList = strOptList + std::to_string(funcIOMap.stat_collision_mode_do[i].type) + std::string(":") +
                         std::to_string(funcIOMap.stat_collision_mode_do[i].index) + std::string(",");
        }
        strOptList = strOptList.replace(strOptList.end() - 1, strOptList.end(), "}");
        ret = ini_file.set("FUNCTIONIO.STAT_COLLISION_MODE_DO", strOptList) && ret;
        //ret = ini_puts("FUNCTIONIO", "STAT_COLLISION_MODE_DO", strOptList.c_str(), filename) && ret;

        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return ret ? 0 : -1;
}

int iniSaveSafetyFuncDIMap(SafetyFuncDIMap sFuncDIMap, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        // std::string strItemList("{");
        // for (int i = 0; i < 8; ++i) { strItemList = strItemList + std::to_string(sFuncDIMap.diChnFunc[i]) + std::string(","); }
        // strItemList = strItemList.replace(strItemList.end() - 1, strItemList.end(), "}");
        // ini_puts("SAFETYFUNCTIONIO", "DI_CHN_FUNC_SETTINGS", strItemList.c_str(), filename);
        Util::IniParser ini_file(filename);
        std::vector<int> v_input;
        for (int i = 0; i < 8; ++i) { v_input.push_back(sFuncDIMap.diChnFunc[i]); }
        ini_file.set("SAFETYFUNCTIONIO.DI_CHN_FUNC_SETTINGS", v_input);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSaveSafetyFuncDOMap(SafetyFuncDOMap sFuncDOMap, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        // std::string strItemList("{");
        // for (int i = 0; i < 8; ++i) { strItemList = strItemList + std::to_string(sFuncDOMap.doChnFunc[i]) + std::string(","); }
        // strItemList = strItemList.replace(strItemList.end() - 1, strItemList.end(), "}");
        // ini_puts("SAFETYFUNCTIONIO", "DO_CHN_FUNC_SETTINGS", strItemList.c_str(), filename);
        Util::IniParser ini_file(filename);
        std::vector<int> v_input;
        for (int i = 0; i < 8; ++i) { v_input.push_back(sFuncDOMap.doChnFunc[i]); }
        ini_file.set("SAFETYFUNCTIONIO.DO_CHN_FUNC_SETTINGS", v_input);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }
    return 0;
}

int iniSaveSafetyRobotPowerLimit(float robotPowerLimit, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        ini_file.set("ROBOTSAFETYSETTING.ROBOT_POWER_LIMIT", robotPowerLimit);
        //ini_putf("ROBOTSAFETYSETTING", "ROBOT_POWER_LIMIT", robotPowerLimit, filename);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }
    return 0;
}

int iniSaveSafetyMomentumLimit(float momentumLimit, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        ini_file.set("ROBOTSAFETYSETTING.MOMENTUM_LIMIT", momentumLimit);
        //ini_putf("ROBOTSAFETYSETTING", "MOMENTUM_LIMIT", momentumLimit, filename);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }
    return 0;
}

int iniSaveSafetyTCPVelLimit(float tcpVelLimit, const char* filename)
{
    if (filename == NULL)
        return -1;

    try
    {
        Util::IniParser ini_file(filename);
        ini_file.set("ROBOTSAFETYSETTING.TCP_VEL_LIMIT", tcpVelLimit);
        //ini_putf("ROBOTSAFETYSETTING", "TCP_VEL_LIMIT", tcpVelLimit, filename);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }
    return 0;
}

int iniSaveCabinetId(unsigned int cabid, const char* filename)
{
    if (filename == NULL)
        return -1;
    try
    {
        Util::IniParser ini_file(filename);
        ini_file.set("ROBOT.CABID", std::to_string(cabid));
        // ini_putl("ROBOT", "CABID", cabid, filename);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }
    return 0;
}

int iniSaveTioPinMode(int pinType, int mode, const char* filename)
{
    if (filename == NULL)
        return -1;
    try
    {
        Util::IniParser ini_file(filename);
        if (pinType == 0)
        {  // DI channel
            ini_file.set("TOOLIOSETTUP.DI_PIN_CFG", mode);
            //ini_putl("TOOLIOSETTUP", "DI_PIN_CFG", mode, filename);
        }
        else if (pinType == 1)
        {  // DO channel
            ini_file.set("TOOLIOSETTUP.DO_PIN_CFG", mode);
            //ini_putl("TOOLIOSETTUP", "DO_PIN_CFG", mode, filename);
        }
        else if (pinType == 2)
        {  // AI channel
            ini_file.set("TOOLIOSETTUP.AI_PIN_CFG", mode);
            //ini_putl("TOOLIOSETTUP", "AI_PIN_CFG", mode, filename);
        }
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }
    return 0;
}

int iniSaveTioRS485ChnMode(int chnId, int mode, const char* filename)
{
    if (filename == NULL)
        return -1;
    try
    {
        Util::IniParser ini_file(filename);
        if (chnId == 0)
        {  // DI channel
            ini_file.set("TOOLIOSETTUP.RS485_CHN1_MODE", mode);
            //ini_putl("TOOLIOSETTUP", "RS485_CHN1_MODE", mode, filename);
        }
        else if (chnId == 1)
        {  // DO channel
            ini_file.set("TOOLIOSETTUP.RS485_CHN2_MODE", mode);
            //ini_putl("TOOLIOSETTUP", "RS485_CHN2_MODE", mode, filename);
        }
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }
    return 0;
}

int iniSaveTioRS485ChnComm(int chnId, ModRtuComm rtuComm, const char* filename)
{
    if (filename == NULL)
        return -1;
    try
    {
        Util::IniParser ini_file(filename);
        std::vector<int> v_input;
        v_input.push_back(rtuComm.slaveId);
        v_input.push_back(rtuComm.baudrate);
        v_input.push_back(rtuComm.databits);
        v_input.push_back(rtuComm.stopbits);
        v_input.push_back(rtuComm.parity);
        // std::string strOptList("{");
        // strOptList = strOptList + std::to_string(rtuComm.slaveId) + ", ";
        // strOptList = strOptList + std::to_string(rtuComm.baudrate) + ", ";
        // strOptList = strOptList + std::to_string(rtuComm.databits) + ", ";
        // strOptList = strOptList + std::to_string(rtuComm.stopbits) + ", ";
        // strOptList = strOptList + std::to_string(rtuComm.parity) + "}";
        if (chnId == 0)
        {  // DI channel
            ini_file.set("TOOLIOSETTUP.RS485_CHN1_COMM", v_input);
            //ini_puts("TOOLIOSETTUP", "RS485_CHN1_COMM", strOptList.c_str(), filename);
        }
        else if (chnId == 1)
        {  // DO channel
            ini_file.set("TOOLIOSETTUP.RS485_CHN2_COMM", v_input);
            //ini_puts("TOOLIOSETTUP", "RS485_CHN2_COMM", strOptList.c_str(), filename);
        }
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }
    return 0;
}

int iniSaveTioVoutEnable(int enable, const char* filename)
{
    if (filename == NULL)
        return -1;
    try
    {
        Util::IniParser ini_file(filename);
        ini_file.set("TOOLIOSETTUP.VOUT_ENABLE", (enable ? 1 : 0));
        //ini_putl("TOOLIOSETTUP", "VOUT_ENABLE", (enable ? 1 : 0), filename);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }
    return 0;
}

int iniSaveTioVoutVoltage(int voltage, const char* filename)
{
    if (filename == NULL)
        return -1;
    try
    {
        Util::IniParser ini_file(filename);
        ini_file.set("TOOLIOSETTUP.VOUT_VOLTAGE", voltage);
        //ini_putl("TOOLIOSETTUP", "VOUT_VOLTAGE", voltage, filename);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }
    return 0;
}

int iniChecksumPersistence(unsigned int crc, const char* filename)
{
    unsigned int ori_crc;
    if (filename == NULL)
        return -1;
    try
    {
        std::string read_value;
        Util::IniParser ini_file(filename);
        ini_file.get("CHECKSUM.SAFETY", read_value, "0");
        ori_crc = std::stoul(read_value);
        // ori_crc = ini_getl("CHECKSUM", "SAFETY", 0, filename);
        if (ori_crc == crc)
        {
            return 0;
        }
        else
        {
            ini_file.set("CHECKSUM.SAFETY", std::to_string(crc));
            //ini_putl("CHECKSUM", "SAFETY", crc, filename);
        }
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }
    return 0;
}

int iniSaveRobotSafety(int model,
                       int custom,
                       int lastCustom,
                       int paramType,
                       float momentumLimit,
                       float tcpVelLimit,
                       float robotPowerLimit,
                       float stopping_dist,
                       float stopping_time)
{
    double limit[5] = {0.0, 0.0, 0.0, 0.0, 0.0};

    if (custom == ZucRobotSafetyCustom::USER_DEFINED)
    {
        limit[0] = momentumLimit;
        limit[1] = tcpVelLimit;
        limit[2] = robotPowerLimit;
        limit[3] = stopping_dist;
        limit[4] = stopping_time;
    }
    else
    {
        std::string modelType = deterRobotModel(model);
        Util::IniParser ini_file(modelType.c_str());
        //ZucIniFile trajInifile;
        // trajInifile.Open(modelType.c_str());
        std::map<int, std::string> section;
        // 添加键值对
        section[1] = "NOLIMITSAFETYSETTING";
        section[2] = "LOOSESAFETYSETTING";
        section[3] = "MEDIUMSAFETYSETTING";
        section[4] = "STRICTSAFETYSETTING";
        std::string node;
        node = section[custom] + ".MOMENTUM_LIMIT";
        ini_file.get(node, limit[0], 70.0, 0, __DBL_MAX__);  //默认使用710 LOOSESAFETYSETTING参数
        node = section[custom] + ".TCP_VEL_LIMIT";
        ini_file.get(node, limit[1], 1620.0, 0, __DBL_MAX__);  //默认使用710 LOOSESAFETYSETTING参数
        node = section[custom] + ".ROBOT_POWER_LIMIT";
        ini_file.get(node, limit[2], 1500.0, 0, __DBL_MAX__);  //默认使用710 LOOSESAFETYSETTING参数
        node = section[custom] + ".STOPPING_DIST";
        ini_file.get(node, limit[3], 500.0, 0, __DBL_MAX__);  //默认使用710 LOOSESAFETYSETTING参数
        node = section[custom] + ".STOPPING_TIME";
        ini_file.get(node, limit[4], 0.4, 0, __DBL_MAX__);  //默认使用710 LOOSESAFETYSETTING参数
        //trajInifile.Find(&limit[0], "MOMENTUM_LIMIT", section[custom - 1].c_str());
        //trajInifile.Find(&limit[1], "TCP_VEL_LIMIT", section[custom - 1].c_str());
        //trajInifile.Find(&limit[2], "ROBOT_POWER_LIMIT", section[custom - 1].c_str());
        //trajInifile.Find(&limit[3], "STOPPING_DIST", section[custom - 1].c_str());
        //trajInifile.Find(&limit[4], "STOPPING_TIME", section[custom - 1].c_str());
    }
    std::string filename = ZUC_USER_CONFIG_DIR "/usersettings.ini";
    Util::IniParser ini_file(filename.c_str());
    ini_file.set("ROBOTSAFETYSETTING.CUSTOM_TYPE", custom);
    ini_file.set("ROBOTSAFETYSETTING.LAST_CUSTOM_TYPE", lastCustom);
    // ini_putf("ROBOTSAFETYSETTING", "CUSTOM_TYPE", custom, filename.c_str());
    // ini_putf("ROBOTSAFETYSETTING", "LAST_CUSTOM_TYPE", lastCustom, filename.c_str());
    rtapi_print("[iniSaveRobotSafety]   limit: %f %f %f %f %f", limit[0], limit[1], limit[2], limit[3], limit[4]);
    switch (paramType)
    {
    case 0:
        ini_file.set("ROBOTSAFETYSETTING.MOMENTUM_LIMIT", limit[0]);
        ini_file.set("ROBOTSAFETYSETTING.TCP_VEL_LIMIT", limit[1]);
        ini_file.set("ROBOTSAFETYSETTING.ROBOT_POWER_LIMIT", limit[2]);
        ini_file.set("ROBOTSAFETYSETTING.STOPPING_DIST", limit[3]);
        ini_file.set("ROBOTSAFETYSETTING.STOPPING_TIME", limit[4]);
        //ini_putf("ROBOTSAFETYSETTING", "MOMENTUM_LIMIT", limit[0], filename.c_str());
        // ini_putf("ROBOTSAFETYSETTING", "TCP_VEL_LIMIT", limit[1], filename.c_str());
        // ini_putf("ROBOTSAFETYSETTING", "ROBOT_POWER_LIMIT", limit[2], filename.c_str());
        // ini_putf("ROBOTSAFETYSETTING", "STOPPING_DIST", limit[3], filename.c_str());
        // ini_putf("ROBOTSAFETYSETTING", "STOPPING_TIME", limit[4], filename.c_str());
        if (custom == 0)
        {
            ini_file.set("ROBOTSAFETYSETTING.OLD_MOMENTUM_LIMIT", limit[0]);
            ini_file.set("ROBOTSAFETYSETTING.OLD_TCP_VEL_LIMIT", limit[1]);
            ini_file.set("ROBOTSAFETYSETTING.OLD_ROBOT_POWER_LIMIT", limit[2]);
            ini_file.set("ROBOTSAFETYSETTING.OLD_STOPPING_DIST", limit[3]);
            ini_file.set("ROBOTSAFETYSETTING.OLD_STOPPING_TIME", limit[4]);
            // ini_putf("ROBOTSAFETYSETTING", "OLD_MOMENTUM_LIMIT", limit[0], filename.c_str());
            // ini_putf("ROBOTSAFETYSETTING", "OLD_TCP_VEL_LIMIT", limit[1], filename.c_str());
            // ini_putf("ROBOTSAFETYSETTING", "OLD_ROBOT_POWER_LIMIT", limit[2], filename.c_str());
            // ini_putf("ROBOTSAFETYSETTING", "OLD_STOPPING_DIST", limit[3], filename.c_str());
            // ini_putf("ROBOTSAFETYSETTING", "OLD_STOPPING_TIME", limit[4], filename.c_str());
        }
        break;
    case ZucRobotSafetyType::ROBOT_MOMENTUM_PARA:
        ini_file.set("ROBOTSAFETYSETTING.MOMENTUM_LIMIT", limit[0]);
        if (custom == 0)
            ini_file.set("ROBOTSAFETYSETTING.OLD_MOMENTUM_LIMIT", limit[0]);
        // ini_putf("ROBOTSAFETYSETTING", "MOMENTUM_LIMIT", limit[0], filename.c_str());
        // if (custom == 0)
        //     ini_putf("ROBOTSAFETYSETTING", "OLD_MOMENTUM_LIMIT", limit[0], filename.c_str());
        break;
    case ZucRobotSafetyType::ROBOT_TCPVEL_PARA:
        ini_file.set("ROBOTSAFETYSETTING.TCP_VEL_LIMIT", limit[1]);
        if (custom == 0)
            ini_file.set("ROBOTSAFETYSETTING.OLD_TCP_VEL_LIMIT", limit[1]);
        // ini_putf("ROBOTSAFETYSETTING", "TCP_VEL_LIMIT", limit[1], filename.c_str());
        // if (custom == 0)
        //     ini_putf("ROBOTSAFETYSETTING", "OLD_TCP_VEL_LIMIT", limit[1], filename.c_str());
        break;
    case ZucRobotSafetyType::ROBOT_POWER_PARA:
        ini_file.set("ROBOTSAFETYSETTING.ROBOT_POWER_LIMIT", limit[2]);
        if (custom == 0)
            ini_file.set("ROBOTSAFETYSETTING.OLD_ROBOT_POWER_LIMIT", limit[2]);
        // ini_putf("ROBOTSAFETYSETTING", "ROBOT_POWER_LIMIT", limit[2], filename.c_str());
        // if (custom == 0)
        //     ini_putf("ROBOTSAFETYSETTING", "OLD_ROBOT_POWER_LIMIT", limit[2], filename.c_str());
        break;
    case ZucRobotSafetyType::ROBOT_STOPPINGDIST_PARA:
        ini_file.set("ROBOTSAFETYSETTING.STOPPING_DIST", limit[3]);
        if (custom == 0)
            ini_file.set("ROBOTSAFETYSETTING.OLD_STOPPING_DIST", limit[3]);
        // ini_putf("ROBOTSAFETYSETTING", "STOPPING_DIST", limit[3], filename.c_str());
        // if (custom == 0)
        //     ini_putf("ROBOTSAFETYSETTING", "OLD_STOPPING_DIST", limit[3], filename.c_str());
        break;
    case ZucRobotSafetyType::ROBOT_STOPPINGTIME_PARA:
        ini_file.set("ROBOTSAFETYSETTING.STOPPING_TIME", limit[4]);
        if (custom == 0)
            ini_file.set("ROBOTSAFETYSETTING.OLD_STOPPING_TIME", limit[4]);
        // ini_putf("ROBOTSAFETYSETTING", "STOPPING_TIME", limit[4], filename.c_str());
        // if (custom == 0)
        //     ini_putf("ROBOTSAFETYSETTING", "OLD_STOPPING_TIME", limit[4], filename.c_str());
        break;
    }
    syncFileToDisk(filename.c_str());

    return 0;
}

int iniSaveReduceSafety(int paramType, reduce_param_config reduce_params)
{
    std::string filename = ZUC_USER_CONFIG_DIR "/usersettings.ini";
    Util::IniParser ini_file(filename.c_str());
    try
    {
        switch (paramType)
        {
        case ZucRobotReducedSafetyType::ROBOT_ALL_REDUCED_REDUCED_PARA:
            ini_file.set("ROBOTSAFETYSETTING.TCP_SPEED_LIMIT_REDUCE", reduce_params.tcp_vel_reduce);
            ini_file.set("ROBOTSAFETYSETTING.ELBOW_VEL_LIMIT_REDUCE", reduce_params.elbow_vel_limit_reduce);
            ini_file.set("ROBOTSAFETYSETTING.MOMENTUM_LIMIT_REDUCE", reduce_params.robot_momentum_limit_reduce);
            ini_file.set("ROBOTSAFETYSETTING.ROBOT_POWER_LIMIT_REDUCE", reduce_params.robot_poewr_limit_reduce);
            ini_file.set("ROBOTSAFETYSETTING.STOPPING_DIST_REDUCE", reduce_params.robot_momentum_limit_reduce);
            ini_file.set("ROBOTSAFETYSETTING.STOPPING_TIME_REDUCE", reduce_params.robot_poewr_limit_reduce);

            // ini_putf("ROBOTSAFETYSETTING", "TCP_SPEED_LIMIT_REDUCE", reduce_params.tcp_vel_reduce, filename.c_str());
            // ini_putf("ROBOTSAFETYSETTING", "ELBOW_VEL_LIMIT_REDUCE", reduce_params.elbow_vel_limit_reduce, filename.c_str());
            // ini_putf("ROBOTSAFETYSETTING", "MOMENTUM_LIMIT_REDUCE", reduce_params.robot_momentum_limit_reduce, filename.c_str());
            // ini_putf("ROBOTSAFETYSETTING", "ROBOT_POWER_LIMIT_REDUCE", reduce_params.robot_poewr_limit_reduce, filename.c_str());
            // ini_putf("ROBOTSAFETYSETTING", "STOPPING_DIST_REDUCE", reduce_params.robot_momentum_limit_reduce, filename.c_str());
            // ini_putf("ROBOTSAFETYSETTING", "STOPPING_TIME_REDUCE", reduce_params.robot_poewr_limit_reduce, filename.c_str());
            break;
        case ZucRobotReducedSafetyType::ROBOT_REDUCED_TCPVEL_PARA:
            ini_file.set("ROBOTSAFETYSETTING.TCP_SPEED_LIMIT_REDUCE", reduce_params.tcp_vel_reduce);
            //ini_putf("ROBOTSAFETYSETTING", "TCP_SPEED_LIMIT_REDUCE", reduce_params.tcp_vel_reduce, filename.c_str());
            break;
        case ZucRobotReducedSafetyType::REBOT_REDUCED_ELBOWVEL_PARA:
            ini_file.set("ROBOTSAFETYSETTING.ELBOW_VEL_LIMIT_REDUCE", reduce_params.elbow_vel_limit_reduce);
            //ini_putf("ROBOTSAFETYSETTING", "ELBOW_VEL_LIMIT_REDUCE", reduce_params.elbow_vel_limit_reduce, filename.c_str());
            break;
        case ZucRobotReducedSafetyType::ROBOT_REDUCED_MOMENTUM_PARA:
            ini_file.set("ROBOTSAFETYSETTING.MOMENTUM_LIMIT_REDUCE", reduce_params.robot_momentum_limit_reduce);
            //ini_putf("ROBOTSAFETYSETTING", "MOMENTUM_LIMIT_REDUCE", reduce_params.robot_momentum_limit_reduce, filename.c_str());
            break;
        case ZucRobotReducedSafetyType::ROBOT_REDUCED_POWER_PARA:
            ini_file.set("ROBOTSAFETYSETTING.ROBOT_POWER_LIMIT_REDUCE", reduce_params.robot_poewr_limit_reduce);
            //ini_putf("ROBOTSAFETYSETTING", "ROBOT_POWER_LIMIT_REDUCE", reduce_params.robot_poewr_limit_reduce, filename.c_str());
            break;
        case ZucRobotReducedSafetyType::ROBOT_REDUCED_STOPPINGDIST_PARA:
            ini_file.set("ROBOTSAFETYSETTING.STOPPING_DIST_REDUCE", reduce_params.robot_momentum_limit_reduce);
            //ini_putf("ROBOTSAFETYSETTING", "STOPPING_DIST_REDUCE", reduce_params.robot_momentum_limit_reduce, filename.c_str());
            break;
        case ZucRobotReducedSafetyType::ROBOT_REDUCED_STOPPINGTIME_PARA:
            ini_file.set("ROBOTSAFETYSETTING.STOPPING_TIME_REDUCE", reduce_params.robot_poewr_limit_reduce);
            //ini_putf("ROBOTSAFETYSETTING", "STOPPING_TIME_REDUCE", reduce_params.robot_poewr_limit_reduce, filename.c_str());
            break;
        }
        syncFileToDisk(filename.c_str());
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSaveDragSpeedLimit(double dragSpeedLimit)
{
    Util::IniParser ini_file(ZUC_USER_CONFIG_FILE(usersettings.ini));
    ini_file.set("ROBOTSAFETYSETTING.DRAG_TCP_SPEED_LIMIT", dragSpeedLimit);

    return 0;
}

int iniSaveJointErrLimit(int jointNum, double err_limit)
{
    if (jointNum < 0 || jointNum >= ZUCMOT_ROBOT_MAX_JOINTS)
    {
        return -1;
    }

    try
    {
        std::string filename = ZUC_USER_CONFIG_DIR "/usersettings.ini";
        std::string node = "JOINT_" + std::to_string(jointNum) + ".JOINT_ERR_LIMIT";
        Util::IniParser ini_file(filename.c_str());
        ini_file.set("ROBOTSAFETYSETTING.STOPPING_TIME_REDUCE", err_limit);
        //ini_putf(item.c_str(), "JOINT_ERR_LIMIT", err_limit, filename.c_str());
        syncFileToDisk(filename.c_str());
    }
    catch (...)
    {
        return -1;
    }
    return 0;
}

int iniSetNetExceptionHandle(float timeLimit, int motType)
{
    std::string filename = ZUC_USER_CONFIG_DIR "/usersettings.ini";
    Util::IniParser ini_file(filename.c_str());
    try
    {
        ini_file.set("NETEXCEPTIONHANDLE.TIMELIMIT", timeLimit);
        //ini_putf("NETEXCEPTIONHANDLE", "TIMELIMIT", timeLimit, filename.c_str());
        switch (motType)
        {
        case MOT_KEEP:
            ini_file.set("NETEXCEPTIONHANDLE.MOTTYPE", "MOT_KEEP");
            //ini_puts("NETEXCEPTIONHANDLE", "MOTTYPE", "MOT_KEEP", filename.c_str());
            break;
        case MOT_PAUSE:
            ini_file.set("NETEXCEPTIONHANDLE.MOTTYPE", "MOT_PAUSE");
            //ini_puts("NETEXCEPTIONHANDLE", "MOTTYPE", "MOT_PAUSE", filename.c_str());
            break;
        case MOT_ABORT:
            ini_file.set("NETEXCEPTIONHANDLE.MOTTYPE", "MOT_ABORT");
            //ini_puts("NETEXCEPTIONHANDLE", "MOTTYPE", "MOT_ABORT", filename.c_str());
            break;
        default:
            return -1;
        }
    }
    catch (...)
    {
        return -1;
    }

    return 0;
}

int iniSaveSimMode(int simEnable, const char* filename)
{
    if (filename == NULL)
        return -1;
    try
    {
        FILE* fptr = fopen(filename, "w+");
        fprintf(fptr, "%d", simEnable);
        fclose(fptr);
        //syncFileToDisk(filename);
    }
    catch (...)
    {
        return -1;
    }
    return 0;
}
